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
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
