LRXWarice
commented
1 year ago 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
robocar2018
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).