Indirect Kalman Filter for Inertial Navigation System

Question

I'm trying to implement an Inertial Navigation System using an Indirect Kalman Filter. I've found many publications and thesis on this topic, but not too much code as example. For my implementation I'm using the Master Thesis available at the following link:

https://fenix.tecnico.ulisboa.pt/downloadFile/395137332405/dissertacao.pdf

As reported at page 47, the measured values from inertial sensors equal the true values plus a series of other terms (bias, scale factors, ...). For my question, let's consider only bias.

So:

Wmeas = Wtrue + BiasW   (Gyro meas)
Ameas = Atrue + BiasA.  (Accelerometer meas)

Therefore,

when I propagate the Mechanization equations (equations 3-29, 3-37 and 3-41) I should use the "true" values, or better:

Wmeas - BiasW
Ameas - BiasA

where BiasW and BiasA are the last available estimation of the bias. Right?

Concerning the update phase of the EKF, if the measurement equation is

dzV = VelGPS_est - VelGPS_meas

the H matrix should have an identity matrix in corrispondence of the velocity error state variables dx(VEL) and 0 elsewhere. Right?

Said that I'm not sure how I have to propagate the state variable after update phase. The propagation of the state variable should be (in my opinion):

POSk|k = POSk|k-1 + dx(POS);
VELk|k = VELk|k-1 + dx(VEL);
...

But this didn't work. Therefore I've tried:

POSk|k = POSk|k-1 - dx(POS);
VELk|k = VELk|k-1 - dx(VEL);

that didn't work too... I tried both solutions, even if in my opinion the "+" should be used. But since both don't work (I have some other error elsewhere) I would ask you if you have any suggestions.

You can see a snippet of code at the following link: http://pastebin.com/aGhKh2ck.

Thanks.

Answer 1

The difficulty you're running into is the difference between the theory and the practice. Taking your code from the snippet instead of the symbolic version in the question:

    % Apply corrections
    Pned = Pned + dx(1:3);
    Vned = Vned + dx(4:6);

In theory when you use the Indirect form you are freely integrating the IMU (that process called the Mechanization in that paper) and occasionally running the IKF to update its correction. In theory the unchecked double integration of the accelerometer produces large (or for cheap MEMS IMUs, enormous) error values in Pned and Vned. That, in turn, causes the IKF to produce correspondingly large values of dx(1:6) as time evolves and the unchecked IMU integration runs farther and farther away from the truth. In theory you then sample your position at any time as Pned +/- dx(1:3) (the sign isn't important -- you can set that up either way). The important part here is that you are not modifying Pned from the IKF because both are running independent from each other and you add them together when you need the answer.

In practice you do not want to take the difference between two enourmous double values because you will lose precision (because many of the bits of the significand were needed to represent the enormous part instead of the precision you want). You have grasped that in practice you want to recursively update Pned on each update. However, when you diverge from the theory this way, you have to take the corresponding (and somewhat unobvious) step of zeroing out your correction value from the IKF state vector. In other words, after you do Pned = Pned + dx(1:3) you have "used" the correction, and you need to balance the equation with dx(1:3) = dx(1:3) - dx(1:3) (simplified: dx(1:3) = 0) so that you don't inadvertently integrate the correction over time.

Why does this work? Why doesn't it mess up the rest of the filter? As it turns out, the KF process covariance P does not actually depend on the state x. It depends on the update function and the process noise Q and so on. So the filter doesn't care what the data is. (Now that's a simplification, because often Q and R include rotation terms, and R might vary based on other state variables, etc, but in those cases you are actually using state from outside the filter (the cumulative position and orientation) not the raw correction values, which have no meaning by themselves).