Society of Robots  Robot Forum

Robot Tutorials

FAQ

Welcome,
Guest
. Please
login
or
register
.
Did you miss your
activation email
?
1 Hour
1 Day
1 Week
1 Month
Forever
Login with username, password and session length
News:
Squirrels have fuzzy tails.
Home
Help
Search
Chat
Login
Register
Society of Robots  Robot Forum
»
Software
»
Software
»
GPS/INS integration using a kalman filter  bias?
Print
Author
Topic: GPS/INS integration using a kalman filter  bias? (Read 2397 times)
0 Members and 1 Guest are viewing this topic.
bunke
Beginner
Posts: 3
Helpful? 0
GPS/INS integration using a kalman filter  bias?
«
on:
March 03, 2011, 08:30:07 AM »
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 ayr*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(:,n1));
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
Logged
mstacho
Supreme Robot
Posts: 376
Helpful? 10
Re: GPS/INS integration using a kalman filter  bias?
«
Reply #1 on:
March 03, 2011, 01:07:06 PM »
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(kk1) = A*P(k1k1)*A' + Qd
is the *predicted* (ie: before estimation) covariance matrix. It is then updated as:
STEP 1: P(kk) = (I  L(k)*C)P(kk1) (and note, I've ignored the idea of your system being timevarying), and P(kk) 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 statespace 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.
Logged
Current project: tactile sensing systems for multifingered robot hands
bunke
Beginner
Posts: 3
Helpful? 0
Re: GPS/INS integration using a kalman filter  bias?
«
Reply #2 on:
March 04, 2011, 03:36:41 AM »
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
Logged
mstacho
Supreme Robot
Posts: 376
Helpful? 10
Re: GPS/INS integration using a kalman filter  bias?
«
Reply #3 on:
March 04, 2011, 07:56:56 AM »
You can think of the two covariance updates (P(kk1) and P(kk)) 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(kk1)  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(kk)  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 timeinvariant and linear!) into a zTransform 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 continuoustime 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 email address so it's harder to read by a robot :P
MIKE
«
Last Edit: March 04, 2011, 01:09:07 PM by mstacho
»
Logged
Current project: tactile sensing systems for multifingered robot hands
bunke
Beginner
Posts: 3
Helpful? 0
Re: GPS/INS integration using a kalman filter  bias?
«
Reply #4 on:
March 04, 2011, 01:19:00 PM »
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.
Logged
Print
Society of Robots  Robot Forum
»
Software
»
Software
»
GPS/INS integration using a kalman filter  bias?
Get Your Ad Here