Why are the state variables passed into the filter not equal to the state variables defined by each [sensor]_config matrix?

Question

I am trying to get a deeper understanding on how ekf_localization_node works.
At the moment I am stuck in interpreting the output of the debug file.

In particular, from the output of that file, I am trying to understand what the line Update indices on the debug file indicates:

Case 1:
Measurement topic name is:

• odom0_twist

  Update indices are:
  [2 3 4 6 7 8 9 10 11 14 ]

[EDIT 1:] Case 2:
Measurement topic name is:

• imu0_acceleration

• imu0_pose

• imu0_twist

  Update indices are: [2 3 4 8 9 10 11 14 ]

From the source code file ekf.cpp I get that the filter checks which measurement state variables actually get passed into the filter. Only if a state variable value is not 'nan' or 'inf' the index is pushed back into updateIndices, which is then printed to the debug file.

I assume that the index value refers to the order/index of the state vector elements. And from the documentation I get the impression that the state vector elements are ordered as follows:

0: position x
1: position y
2: position z
3: rotation roll
4: rotation pitch
5: rotation yaw
6: velocity X
7: velocity Y
8: velocity Z
9: velocity roll
10: velocity pitch
11: velocity yaw
12: acceleration X
13: acceleration Y
14: acceleration Z

Therefore I conclude that the line Update indices from the debug file is telling me that following state variable are passed into the filter: 2:position z , 3:rotation roll, 4:rotation pitch, 6: velocity X , 8: velocity Z, 9: velocity roll etc..

This does not match with the [sensor]_config matrix (a.k.a true/false matrix) values that specify what variables should be fused into the final state estimate (see configuration file below).

Could some help me to understand what the line Update indices on the debug file indicates?

[EDIT 2:]

Code comments confirm that 'Update indices', in effect, describes which state variables that are passed into the filter.

The question that remains is:
Why are the state variables passed into the filter not equal to the state variables defined by each [sensor]_config matrix?

# My configuration for the robot_localization EKF node
frequency: 10

two_d_mode: true
debug: true
debug_out_file: /home/rob/catkin_ws/src/ekf/ekf_debug_file.txt
publish_tf: false

odom_frame: odom
base_link_frame: base_link
world_frame: odom
# map_frame: map


odom0: /odom_noisy
odom0_config: [false, false, false,
               false, false, false,
               true,  true,  false,
               false, false, true,
               false, false, false]


odom0_differential: false


imu0: /mobile_base/sensors/imu_data_raw

imu0_config: [false, false, false,
              false, false, true,
              false,  false,  false,
              false, false, true,
              true, false, false]

imu0_differential: false

---

Originally posted by Roberto Z. on ROS Answers with karma: 500 on 2020-09-09

Post score: 1

Answer 1

There are two reasons:

1. If your raw sensor data is given in a different coordinate frame than the EKF uses, the matrix in the config specifies what data to use in the sensor frame, and the raw filter is using the indices in the target frame (body frame or world frame, depending on what you're fusing).
2. If you have two_d_mode on, then for every measurement you get on a given sensor, we always fuse a 0 value for all 3D variables. If we didn't do this, your state estimate would quickly diverge. So looking at your odom example, you have X velocity, Y velocity, and yaw velocity enabled. Since you also have two_d_mode on, I would expect Z position, Z velocity, roll, pitch, roll velocity, pitch velocity, and Z acceleration to all get measured too (again, we just pass it a measurement of 0 with a tiny covariance). Here are the indices you posted:

[2        3        4        6        7        8        9        10       11       14       ]

Note that those indices match expectations.

---

Originally posted by Tom Moore with karma: 13689 on 2020-09-29

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by Roberto Z. on 2020-10-16:
I can confirm that setting two_d_mode: true is what causes to include all 3D-space state variables (Z, roll, pitch, their respective velocities, and Z acceleration) to the filter.

I was expecting that two_d_mode: true would cause 3D-space state variables to be ignored. The reason being that the system of linear equations depends only on the state variables of interest. In fact if I set two_d_mode: false less state variables are considered -> smaller sub-matrices -> less calculations steps required by each filter iteration.

Comment by Tom Moore on 2020-10-16:
Right, but that requires a separate kinematic model, and we only have one, and it does estimation in full 3D. I painted myself into a corner when I designed it, and adding other kinematic models isn't trivial. I simply don't have the cycles to add that feature.