GPS/INS integration using a kalman filter - bias?

Discussion in 'Math' started by bunke, Mar 3, 2011.

  1. bunke

    Thread Starter New Member

    Mar 3, 2011
    1
    0
    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
     
Loading...