Eq. 6 of Brossard's paper: Rotation matrix must be confined in x-y plane.

[scott81321]

I have encountered a problem with Brossard's equations, namely eq. 6, and written in his
code in utils_numpy_filter.py, specifically in the 'propagate' routine:

acc = Rot_prev.dot(u[3:6] - b_acc_prev) + self.g

where b_acc_prev includes the biases and the noises.

The problem is that the rotation matrix has to be confined in the x-y plane to ensure that
the accel data u[3:6] i.e. (ax,ay,az) where az is around 9.8 m/sec^2 is exactly canceled by the gravity vector (self.g). Otherwise, the gravity component 'az' can be rotated into the x-y plane causing stray accelerations and runaway solutions in the case of vehicles moving on a flat x-y plane. I have encountered such a case.

In my test run, my initial rotation matrix is indeed confined to the x-y plane as initial pitch and roll are zero. However, during the propagation it is multiplied by the exponential of SO(3) on \omega*dt i.e. eq. 5. Now for small angles, this matrix is an identity matrix with small corrections. However, for large \omega i.e. big gyroscope measures as in gyro_z which is significant during a turn, this end result is no longer confined to the x-y plane.

Should this equation be modified to acc = Rot_prev.dot(u[3:6] - b_acc_prev + self.g)
i.e. have gravity exactly cancel u[6] and then multiply the whole thing by the rotation matrix?

This issue genuinely scares me because it's at the core of the implementation for ai-imu-dr.
IMHO There is conceptually nothing wrong with eq. 6, e.g. in the case of an airplane which
does feel gravity but for a vehicle moving on a x-y plane on planet Earth, gravity is cancelled out (by the normal force from the ground).

[LRXWarice]

In my opinion Eq (6) is correct.
You have to distinguish between IMU frame and global frame. The global frame of the KITTI Dataset, which is used in this paper, is a ENU CS I think (x to East, y to North, z Up). The IMU frame (x to front, y to left, z up) however, can be rotated freely. You are right, for cars the rotations that are most common are performed around the z axis (yaw). However, also pitch and roll can occur, e.g. driving up a hill.
In Eq (6), a_n (u[3:6]) are the measurements in the IMU frame. If the device is positioned on a flat surface (roll and pitch = 0) and not accelerated, you will have following measurement: a_n = [0, 0, 9.8065] (assuming no noise and bias).
If the device it tilted around the y axis with 10° (pitch), the effects of gravity would be measured by z and x axis: a_n = [-1.7, 0, 9.65].
So in order to subtract the gravity vector g = [0,0,9.8065], you need to transform the measurements from the (rotated) IMU frame to the global frame, since here, the z axis is parallel to the gravity vector:
a_global = R*a_n. , in the first case (roll, pitch,yaw = 0), R = identity matrix
a_global = [0, 0, 9.8065] in both cases
now you can cancel gravitational effect:
a_corrected = a_global - g
The corrected global acceleration in the world frame can subsequently be integrated to calculate velocity & position in the global cs.

When you would modify the equation to this: acc = Rot_prev.dot(u[3:6] - b_acc_prev + self.g),
you do not cancel the measurements caused by gravity correctly if the IMU is even slightly rotated, since g is [0,0,9.8065].

By the way, it doesn't matter if the IMU is mounted to a car or plane that for instance moves parallel to earths surface. In both cases the IMU measures the effects of the gravitational force equally. And you have to compensate it in both cases.

I think the issue you are experiencing in your test run has to do with the noise and bias of your IMU. You are indeed correct, that an error in orientation causes projection of the gravitational force onto the horizontal axes in the world frame. If your gyroscope has noise and bias that cannot be compensated, you end up with incorrect orientation (eq (5)). When you use the incorrect Rotational matrix in eq(6), thats exactly what ends up happening. This is the main cause for drift in MEMS IMUs when estimating a position. The IMU used for the KITTI dataset is expensive and has small noise and bias. That's why you can integrate the raw measurements from the dataset and still get a fairly accurate position within a certain time interval. If you try to do this with a cheap IMU this does not work and you will see huge drift even after a few seconds.
So i think the noise and bias of your IMU causese the issues you described.

[scott81321]

@LRXWarice Thank you. I believe you are mostly right and that eq. 6 is correct after all. Since I posted my earlier message, I have done more testing trying to find the root cause of my problem. Yes, KITTI is ENU and so is ai-imu-dr though the test cases only show plots in the E-N plane. The 'z' axis is up and so accel_z registers gravity in full.

My suggestion acc = Rot_prev.dot(u[3:6] - b_acc_prev + self.g) was only a hack that worked for a certain troublesome path I describe further on but all my other test cases fail as well if I use this hack. These same test cases work well enough with eq. 6 as it stands. I also tried extracting the yaw from Rot_prev and 'restore' Rot_prev from only the yaw and thus ensuring Rot_prev.dot(self.g) = self.g

Brossard's program works beautifully for a travel on a winding road with some elevation up and down all the way to an underpass where GPS fails. The vehicle then goes under an underpass and stops at a traffic light for about a minute until it returns. For the travel to and from that underpass, the program does beautifully.

=> Where it goes wrong is that period where the vehicle has stopped and rather than record zero velocity, it has velocity with constant acceleration and a runaway solution! It fails there and I found no noise-covariance choice to address that. The hacks I mentioned are the only things I found that could eliminate this apparent drift acceleration and solution.

The rotation matrix Rot_prev is a key issue I believe.

After much analysis of the solutions, I get the impression that the Rotation matrix Rot_prev had degraded somewhat even before entering the underpass after a considerable gradual turn. Once the vehicle stopped inside the underpass: there was no more 'juice' i.e gyro data from which to renew Rot_prev. So Rot_prev was 'stuck' in a bad mode and generated spurious accelerations via eq. 6. As you might know, Rot_prev is compounded at each iteration with the SO3 routine. Rot_prev is in eqs.5-6-7 and is what drives the propagation of the vehicle IMHO. The program has routines to address numerical degradation of the rotation matrices using SVD but these are not enough here apparently.

I could use some suggestions here.

BTW, considering the body frame, @milkcoffee365 opened an issue on Jun 23, 2022 concerning eq. 14 of Brossard's paper. The early reported claimed a mistake in the code. Recent messages mentioned discrepancies between Brossard's paper in arxiv and Hal vs the IEEE publication. There were errors concerning Eq. 14 and 37. However, fixing these did not solve my problem.

@robocar2018 has been helpful in making me understand that this algo is not designed
for e.g. a vehicle making a trek and then stop for some 60 minutes or so. If the rotation matrix has degraded then the algo will be in a bad state and the stop will yield spurious accelerations and runaway solutions, as I have. One needs ZUPT (Zero Velocity Potential Update ) I found a fix to that. However, I am seeking a means of fixing the rotation matrix as it degrades. The angle between R*g and 'g' can serve as a criteria but I need to know how to fix it - if that is possible.

[robocar2018]

@scott81321 , as long as you can reliably tell the vehicle is static, then fixing the rotation matrix is not difficult. You can compute pitch and roll from the gravity's decomposition into the IMU's body frame. And as for the yaw, you can fix it as previous one.
An example would be like : https://wiki.dfrobot.com/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
But my concern is if you are using IMU to reliably predict whether the vehicle stops, then you will introduce some delays due to averaging the IMU data, within such delay, would the R matrix drift?

[scott81321]

@robocar2018 Thank you. What I do for the time being is when I get to a IMU accel+gyro data point for a time 't' I then average accel data over the period t+delta_t where delta_t corresponds to 400 points i.e. 4 seconds with a sampling frequency of 0.01 seconds or 100 Hz. I can 'look ahead' in this way with Brossard's program because the entire IMU input is in a pre-processed pickle file anyway. This avoids the delays but granted this looking ahead is not satisfactory and only works if you have a moving buffer of data. A better solution is a machine learning option where the data collected for when the vehicle is at a stop is used to train the machine so that I can know at each and any time 't' whether the vehicle has stopped.

Concerning the degradation of the rotation matrix in my test case, I find that as it degrades, something like 90% of the angle between R*g and g is in the pitch part of the Rotation matrix. If I take the pitch out of the rotation matrix R, I get considerably better results. This is odd because from our video, the vehicle is traveling on a flat surface. Go figure. Any ideas? Is it possibly a question of the sensors' accuracy? (I am not using OXTS) But if so, why the devolution in the pitch? IMU accel_z assures us that gravity is always in the 'z' - up direction for the test case. I know I have asked this before but are you aware of any other errors in the equations of Brossard's paper (and/or code)?

[robocar2018]

@scott81321 , rotation matrix degradation is hard to fix in the dead-reckoning since we don't have external sensors, and such degradation is caused by the gyro bias and noise, you can't estimate the gryo bias and noise perfectly.

[scott81321]

@robocar2018 I have found out more about why removing the pitch (P) from the rotation matrix before two particular stops for a period where the speed reaches its peak and descends to zero, greatly improved the results of ai-imu-dr. I think I am getting a picture. The P in the rotation matrix in the propagate routine was found to be strongly correlated to b_acc_x and b_acc_y and b_omega_y (without my ZUPT fixes). Now b_omega_y concerns gyro_y ie. rotation about the y axis and affecting the x-z plane. As we integrate \omega * dt with the so3exp routine, because of the significant b_omega_y, it's the 'x' and 'z' components of the rotation matrix that will be considerably affected much like a rotation about the 'y' axis (compare roty to so3exp). Thus, the degradation of the rotation matrix is degraded according to pitch. Mind you, in all this, though I get correlations, I do not yet get a sense of causality i.e. what caused what? Does the degradation of the noise covariances/biases affect the rotation matrix or the other way around? I don't know if I am exploiting a previously unknown property or if there is something still missing to the "upgrade" routine in spite of your fixes. Can you please make your derivations and corrections public? or just email them to me at tcscott@gmail.com