GPS/INS integration using a kalman filter - bias?

[bunke]

Hey everybody.

I am trying to estimate the latereal velocity and accelerometer bias using a kinematic kalman filter. I do this as follows:

The states are [Vy, a_bias];
Vy=lateral velocity, a_bias=bias of the accelerometer.

The input is ay-r*Vx; ay=lateral acceleration measurement, r=yawrate, Vx =longitudal velocity

The gps velocity is used as a measurement. However the INS samples faster than gps.
System matrices
A=[0 -1,0 0];
B=[1;0];
C=[1 0]; When gps is available
C=[0 0]; When gps is not available.


while k<length(Vgps)-1 %

    %measurement update
    if(abs(Vgps(k,1)-ay(n,1))<abs(Vgps(k,1)-ay(n+1,1))) %is Gps is available then update measurements
        L=P*C'*(C*P*C'+R)^-1;
        X(:,n)=X(:,n)+L*(B_hat(n)*Vgps(k,2)-C*X(:,n));
        P=(eye(2)-L*C)*P;
        k=k+1;
    end
   
    %Time update
    dX(:,n)=A*X(:,n)+B*(ay(n,2)-YawRate(n,2)*Vgps(k,2)*cos(B_hat(n)));
    if(n>1)
        X(:,n)=X(:,n)+Tsgyro/2*(dX(:,n)+dX(:,n-1));
    else
        X(:,n)=X(:,n)+Tsgyro/2*(dX(:,n));
    end
     P=A*P*A'+Qd;

B_hat(n)=asin(X(1,n)/(Vgps(k,2)*cos(B_hat(n))));
n=n+1;
end

In this
B_hat    = sideslip estimate
Vgps     = gps velocity
Tsgyro  = sampletime for gyro/accelerometer
P          = State estimation covariance matrix
Qd        = discretized process noise matrix
R          = Sensor noise matrix
L          = Kalman gain

This filter has a trapizoidal integration scheme.
Two questions:
The paper from which i nicked this says that A the state covariance matrix P=A*P*A'+Qd is supposed to be the discretized jaobian at each timestep. But since A is just a constant isent it just the same thing?

How is it supposed to estimate the bias? The zero in the C matrix prohibits that it is updated by the measurements update and it doesent grow in the time  update. When I run the scipt the bias is zero. Am I missing something? Any help would be greatly appreciated since I am throughly stumped.

Kind regards
Bunke

[mstacho]

I'd have to see the paper you took this all from.  I assume that you have a nonzero bias on the accelerometer during your simulation? :-P  It is a good question as to how it can estimate the bias (it doesn't look like it can, as far as I can tell...I'll have to think about it)

BUT as far as it seems to me, you haven't implemented the kalman filter properly (or you left out a lot of detail that lead me to believe that :-P )

P = A*P*A' + Qd is more properly:

STEP 1: P(k|k-1) = A*P(k-1|k-1)*A' + Qd

is the *predicted* (ie: before estimation) covariance matrix.  It is then updated as:

STEP 1: P(k|k) = (I - L(k)*C)P(k|k-1) (and note, I've ignored the idea of your system being time-varying), and P(k|k) goes back into STEP 1 at the next time step.

In other words, no, it's not the same even if A is constant, since you're continually using a predicted covariance to update the actual ("a posteriori") covariance estimate.  Technically you should always have two covariance matrices going at one time, the prior and the posterior.

Now in terms of the biases...your state equation isn't correct for the type of kalman filter you seem to be implementing.  There are two types of KF: continuous and discrete.  Never use continuous  (in reality, you of course CAN use it, but it's...really hard to do).  The easier of the two to implement is the discrete KF, but that means you have to discretize your state-space model.  It isn't just that xDot = Ax + Bu, and you use a numerical integration scheme to approximate it -- that IS NOT a discretized version of the model, it's an *approximate* version of it.

If you're using MATLAB, and you have the control systems toolbox, learn how to use "c2d", and that will give you the discretized version of the model to use.

[bunke]

Hi
Thank you for replying. Yes there is a very disctint bias on the measurement.

Okay so I should discretize the A matrixes in the system? Done.

I have to different updates for the state covariance matrix. One in the measurement update and one in the time update. The one in the time update has so far been wrong since i needed the discrete A matrix.  But they are two different matrices? Or have I gotten the wrong idea about how they update.

I tried appending the paper but its to large. Would you be willing to take a lot at it if i sent it to you?
Kind regards
bunke

[mstacho]

You can think of the two covariance updates (P(k|k-1) and P(k|k)) as the "early" and "after" versions of one matrix if you prefer.  The first is generated using its incarnation from the previous time step along with the A matrix and plant noise.  The second uses the first and the newly computed Kalman gain to get a better estimate of the covariance *after* the optimal (aka Kalman) gain has been computed, given what you now know about the system in this time step.

It works sort of like this:  If you see something off in the distance, you're not sure what it is, but you make a guess.  That guess is P(k|k-1) -- it's the covariance you THINK is correct in the current time step, given the covariance you had from the previous.

If you now move toward the object slightly, you can see it better (you take a new measurement), and you're better able to make the guess.  That is P(k|k) -- the new guess, given the new information.  The way you generate that new guess is by computing the kalman gain, which is the optimal gain (in the sense that it minimizes mean squared error of estimation between the actual and estimated state).

As long as that's what you've done, you're good to go :-P

Although it's probably a terminology issue between us, you don't just discretize the "A matrices" ofthe system.  you discretize the whole system.  So in matlab, I think the command is:  c2d([A,B,C,D],dt).  This gives you the discretized system with the time step dt.  This turns the system from a Laplace Transform (if it's time-invariant and linear!) into a z-Transform representation.  The two are NOT equivalent, so keep that in mind, but they behave in a similar way, and using the discretized version allows you to ignore the pains that come with the continuous-time kalman filter.

Sure, send the paper to mstachowsky [At SigN] gmail (dot] com.  I'll take a look -- I'll be doing GPS/INS integration soon anyway, so it's always good to read more :-)

**edit: changed the e-mail address so it's harder to read by a robot :-P

MIKE

[bunke]

Hi everybody

The question still remains. How does the this filter estimate the bias? If needed I can send the paper I'm using. I really would appreciate the help.