Question about IMU dynamics input in AttitudeObserver

[mmurooka]

This is not an Issue but a question. I am trying to understand AttitudeObserver to get a clue to solve https://github.com/jrl-umi3218/mc_state_observation/issues/6

My question is about the following lines. https://github.com/jrl-umi3218/mc_state_observation/blob/18175893d394bfb219c3cb18373ff0510af2a4f0/src/AttitudeObserver.cpp#L128-L130

Am I correct in understanding that the input to the IMU dynamics (uk_) is a jerk of position and orientation? If so, how was the above equation to calculate the jerk derived/determined? The coefficient matrix has a fixed value, but does it need to be tuned/configured?

https://github.com/jrl-umi3218/mc_state_observation/blob/18175893d394bfb219c3cb18373ff0510af2a4f0/src/AttitudeObserver.cpp#L25-L28

CC @mehdi-benallegue

[mmurooka]

Another question is about the value of the covariance matrix q_ of the noise in the state dynamics. In my understanding, the state dynamics updates the position, velocity, and acceleration states of the position and orientation from the jerk inputs by just integrating them. Despite such a simple integration dynamics, different default values of covariance matrix are set for each component. Is there a rationale for this?

https://github.com/jrl-umi3218/mc_state_observation/blob/18175893d394bfb219c3cb18373ff0510af2a4f0/include/mc_state_observation/AttitudeObserver.h#L33-L35

[mehdi-benallegue]

Thank you very much for these questions, they should be better documented, the reason it has not been done is that this implementation was copied from hrpsys-state-observation, which was copied from a unit test in state-observation. My current plan is to put this implementation back as a standard estimator in state observation and have only simple interfaces in this library.

This state observer needs to observe the 6D kinematics of the IMU together with their derivatives up to the second order, this state is then 18th dimensional. This state is actually not observable. What is observable at best is only two dimensions of orientation (tilt), three dimensions of angular velocity and acceleration, and three dimensions of linear acceleration which amounts at 11 dimensions (depending on the trajectory, it may be less). Since the system is marginally stable, if I try to use this system as is for observation, it would drift quadratically and diverge.

Therefore I need to cheat by making the system artificially stable in unobservable modes to avoid divergence. One solution could be to make the system dynamics stable intrinsically. However, if I do that, Kalman filtering would try to exploit these artificial dynamics to make it artificially observable and this would degrade the quality of the observation. The solution I chose was to add inputs that stabilize the system in a way that does not modify the (marginally stable) dynamics that Kalman use (Kalman has no idea on how these inputs are computed), while still making the system stable. This wat should allow being reactive to high accelerations that are not negligible with regard to gravity even if it is not just an impulsion. For instance, if there is an acceleration that is not aligned with the gravity field, I want Kalman to first consider it as linear acceleration, but if the same acceleration is sustained, I want Kalman to consider that it is less and less likely that it is a linear acceleration, but rather gravity that is not properly aligned. I want this drop in "likelihood" to be linear in time and not exponential (like in complementary filtering) which I consider too fast. I am not sure if it is clear.

So this is the objective. However, at one moment, there was a mistake that was introduced, the input was designed first to be the acceleration and not the jerk, but if I remember well, it was too conflicting with the dynamics of the accelerometer. So I changed it to be the jerk, but I don't know how I did not reflect that in the estimator. When I realized this mistake three months ago, I fixed it in the PR that I proposed (https://github.com/jrl-umi3218/mc_state_observation/pull/17) and that I need to investigate better (it seems that it is not working for Arnaud, and I don't know why yet).

These is the new dynamics in the PR https://github.com/jrl-umi3218/mc_state_observation/blob/b7ee02fe84e11890cd6b09680932b0ac75d60257/src/AttitudeObserver.cpp#L171-L175

What is funny is that it still mostly works despite this error, probably because the introduced unstable mode is observable so it does not diverge, but I did not study it further.

To reply to your other question, these are parameters that we can tune, but I don't expect their values to have a huge effect, this is mainly because they are small, and because sensors should have most of the importance, together with the dynamics linking them to the observable state.

Regarding your last question, the values of the covariances were tuned to have the best performances, I agree that the value for linearAccCov is strange, I don't remember the reason. But the idea is to not give a lot of trust to this jerk-stabilization and to correct only when the error becomes big (which means that probably the orientation is misaligned).

Sorry for the slow progress in maintaining this estimator. It is a bit hard to modify this observer since it works on most robots much better than the previous kalman filter (especially those with good gyrometers), and it requires a lot of tests to validate any change. I even have a multiplicative Kalman filter version implemented, with much higher computational performance (about the same observation quality), that should replace this one, if I had time to finalize it.

[mmurooka]

Thanks for the reply. It is now clear.

Just out of curiosity, is the following trick common in the field of state estimation or is it your original idea?

The solution I chose was to add inputs that stabilize the system in a way that does not modify the (marginally stable) dynamics that Kalman use (Kalman has no idea on how these inputs are computed), while still making the system stable.

[mehdi-benallegue]

As far as I know, nobody does that. A classic solution for this kind of issue is to add pseudo measurements

in this case, if I had to choose this option I would set pseudo measurements that the position and orientation are at the origin with low confidence. This solution could have a good effect but I did not want to make the system bigger (adding inputs does not increase the Kalman problem).

[mmurooka]

I see. This is an interesting trick. A comparison of pseudo measurements vs pseudo inputs (in terms of computational cost and estimation results, etc.) would be interesting.

I'll close this now that my question has been answered.

[mmurooka]

@mehdi-benallegue Let me add one more question. In the following lines, matrices that linearly approximate the dynamics are computed using the finite difference method. However, the dynamics is simple enough to allow us to derive analytical derivatives (although it is a bit complicated due to the inclusion of the rotation matrices). I understand that analytical derivatives are generally preferable to finite difference in terms of computational cost and numerical error. Could you comment something about how you came to use finite difference?

https://github.com/jrl-umi3218/mc_state_observation/blob/24f5a8db63eaaa3a763c1358dbacf57dd1820206/src/AttitudeObserver.cpp#L142-L146 https://github.com/jrl-umi3218/state-observation/blob/3d7fabcddcd5c9d7669bf93f5401fd1f145c7e91/src/extended-kalman-filter.cpp#L145-L181

[mehdi-benallegue]

Thank you again for your question.

You are right, the dynamics are not that complicated, except that in these dynamics the orientation is represented with an angle * axis representation. It has the advantage of being of minimal size and not having to respect any constraint. The drawback is that it is quite complicated to compute the derivative of the integration scheme of this representation (i.e. we know the current orientation, and we want to know the jacobian of the new orientation with regard to angular velocities). I tried several times and every time it was way more complicated than just finite differences.

The real solution to this problem is to consider that the state lies in a Lie group, and to write the jacobians of the dynamics of the state and the sensors in the associated Lie Algebra at the origin. This makes the representation of the orientation irrelevant (could be a quaternion or even the whole rotation matrix) and the jacobians much easier to compute. This amounts also at using the multiplications of quaternions (and inverse of quaternions) instead of additions and subtractions it is called a multiplicative Kalman Filter (we use also exponential maps and logs for moving to the Lie Algebra). In some case you can write the errors in a way that makes the jacobians invariant with regard to the state and it is called Invariant Kalman Filter, but I don't think we can make it invariant.

This multiplicative Kalman Filter has been implemented as stated in this above message: https://github.com/jrl-umi3218/mc_state_observation/issues/21#issuecomment-1328198747 and it does show a much greater computational efficiency. However, no substantial improvement in estimation quality was noticeable in my tests. Nevertheless, I agree that we must switch to this formulation but this was not very high on my to-do list. Actually, it was supposed to be the next task I would assign Arnaud Demont when he submits his paper.

Here is an example of implementation of the multiplicative Kalman filter for the IMU state-observation/examples/imu-multiplicative-attitude-reconstruction.hpp With its use in a unit test (the same data as for the non multiplicative one) state-observation/unit-testings/imu-multiplicative-test.cpp

[mmurooka]

I see. Thank you very much for your explanation!