I have some questions regarding a Kalman filter design and implementation. I am new to Kalman filters so I apologize if my questions are too basic. I also apologize for my fairly long post. My application is an autonomous vehicle. I need estimates of x,y,z position and velocity in world frame. I have measurements from multiple sensors giving me z in world frame and x,y,z velocities and accelerations in body frame. I also have an estimate of the attitude in Euler angles from other sensors which gives me a direction cosine to convert from body to world frame. So given the Kalman state and measurement equations: x(k) = A(k-1)x(k-1) + w(k-1) z(k) = H(k)x(k) + v(k) Here is my filter model, given that I am trying to initially use a simple (not extended) Kalman filter. My x(k) is a 9x1 vector of the positions, velocities and accelerations in world frame: i,e x(k) is [ Xp_wf ] [ Yp_wf ] [ Zp_wf ] [ Xv_wf ] [ Yv_wf ] [ Zv_wf ] [ Xa_wf ] [ Ya_wf ] [ Za_wf ] Then A(k) follows from the kinematics as: [ 1 0 0 dt 0 0 0.5*dt*dt 0 0 ] [ 0 1 0 0 dt 0 0 0.5*dt*dt 0 ] [ 0 0 1 0 0 dt 0 0 0.5*dt*dt] [ 0 0 0 1 0 0 dt 0 0 ] [ 0 0 0 0 1 0 0 dt 0 ] [ 0 0 0 0 0 1 0 0 dt ] [ 0 0 0 0 0 0 1 0 0 ] [ 0 0 0 0 0 0 0 1 0 ] [ 0 0 0 0 0 0 0 0 1 ] where dt is time step. My measurement vector z(k) is a 7x1 vector consisting of the z position in world frame and the velocities and accelerations in body frame i.e. z(k) is [ MZp_wf ] [ MXv_bf ] [ MYv_bf ] [ MZv_bf ] [ MXa_bf ] [ MYa_bf ] [ MZa_bf ] The H(k) matrix is fairly straightforward but is a little cumbersome to list so I will just say that it converts the world frame velocities and accelerations in the state vector to the body frame velocites and acceleration in the measurement vector using the direction cosine matrix. The Z position measurement is a 1:1 mapping to the Z position in the state vector. OK now on to some questions about the design: - Is this the best way to model the state and measurement, given what I want an estimate of? I have seen examples where the accelerations are not used in the state vector but as u(k) a driving function in the state equation. I am not sure how a measurement(acceleration) can be used in the state equation. - Given that the direction cosine matrix will in effect be derived from another Kalman filter estimate of the Euler angles, is it valid to use the direction cosine in the H matrix? - Given that the acceleration measurements come from accelerometers, how much should I worry about accelerometer bias and drift(which does exist) as well subtracting out gravity or will the Kalman filter minimize its effect. Now on to the implementation. The Kalman filter time update equations project the state and covariance from the aposteriori estimates from the last time step: Xest_apri(k+1) = A(k)Xest_apost(k) P_apri(k+1) = A(k)P_apost(k)A_T(k) + Q(k) and the Kalman filter measurement update equations update the estimates from the apriori estimates of the last time step: K(k) = P_apri(k)H_T(k) Inv(H(k)P_apri(k)H_T(k) + R(k)) Xest_apost(k) = Xest_apri(k) + K(k)(Z(k) - H(k)Xest_apri(k)) P_apost(k) = (I - K(k)H(k)) P_apri(k) In my application, different sensor data arrive asynchronously at different times. I however need the state estimates at a higher rate synchronously to drive other parts of my control system. So some questions I had on the implementations are: - My understanding from the above equations are that when a measurement arrives, the time update equations need to be run to project the apriori estimates for the current measurement time from the last aposteriori estimate and then the measurement update equations need to be run to get the new aposteriori estimate. When I need estimates for my synchronous loop, only the time update equations need to be run. So I run both time update and measurement update equations when measurements arrive and only the time update equation when a estimate is desired for my synchronous loop. Is this correct? - Given that what I stated in my last question is true, if no measurement arrives between successive synchronous updates, would I use the last aposteriori estimate from the last measurement update in my time update equation to generate the second synchronous update? That is, would I use the time update equation successively but with the same aposteriori estimates for X and P. - I am a little confused about the measurement update equations. Given that when a sensor update arrives, it only updates part of the measurement vector z(k). For example, I have a sensor that updates the x,y,z velocities in body frame? I don't have a new z position and x,y,z acceleration update at this time. Do I still run the full measurement update equation, even though only part of the measurement vector is new? It seems counter intuitive to me since it seems you are telling the filter that for part of the measurement vector there is new data when there really isn't. There also doesn't seem a way to break up the state and measurement vectors into smaller units and separate filters so that this problem doesn't arise. Any comments? Thanks TIA
Kalman filter help
Started by ●February 12, 2004
Reply by ●February 13, 20042004-02-13
ta wrote:> > I have some questions regarding a Kalman filter design and > implementation. I am new to Kalman filters so I apologize if my > questions are too basic. I also apologize for my fairly long post. > > My application is an autonomous vehicle. I need estimates of x,y,z > position and velocity in world frame. I have measurements from > multiple sensors giving me z in world frame and x,y,z velocities and > accelerations in body frame. I also have an estimate of the attitude > in Euler angles from other sensors which gives me a direction cosine > to convert from body to world frame. > > So given the Kalman state and measurement equations: > > x(k) = A(k-1)x(k-1) + w(k-1) > z(k) = H(k)x(k) + v(k) > > Here is my filter model, given that I am trying to initially use a > simple (not extended) Kalman filter. My x(k) is a 9x1 vector of the > positions, velocities and accelerations in world frame: i,e x(k) is > > [ Xp_wf ] > [ Yp_wf ] > [ Zp_wf ] > [ Xv_wf ] > [ Yv_wf ] > [ Zv_wf ] > [ Xa_wf ] > [ Ya_wf ] > [ Za_wf ] > > Then A(k) follows from the kinematics as: > > [ 1 0 0 dt 0 0 0.5*dt*dt 0 0 ] > [ 0 1 0 0 dt 0 0 0.5*dt*dt 0 ] > [ 0 0 1 0 0 dt 0 0 0.5*dt*dt] > [ 0 0 0 1 0 0 dt 0 0 ] > [ 0 0 0 0 1 0 0 dt 0 ] > [ 0 0 0 0 0 1 0 0 dt ] > [ 0 0 0 0 0 0 1 0 0 ] > [ 0 0 0 0 0 0 0 1 0 ] > [ 0 0 0 0 0 0 0 0 1 ] > > where dt is time step. > > My measurement vector z(k) is a 7x1 vector consisting of the z > position in world frame and the velocities and accelerations in body > frame i.e. z(k) is > > [ MZp_wf ] > [ MXv_bf ] > [ MYv_bf ] > [ MZv_bf ] > [ MXa_bf ] > [ MYa_bf ] > [ MZa_bf ] > > The H(k) matrix is fairly straightforward but is a little cumbersome > to list so I will just say that it converts the world frame velocities > and accelerations in the state vector to the body frame velocites and > acceleration in the measurement vector using the direction cosine > matrix. The Z position measurement is a 1:1 mapping to the Z position > in the state vector. > > OK now on to some questions about the design: > > - Is this the best way to model the state and measurement, given what > I want an estimate of? I have seen examples where the accelerations > are not used in the state vector but as u(k) a driving function in the > state equation. I am not sure how a measurement(acceleration) can be > used in the state equation. > > - Given that the direction cosine matrix will in effect be derived > from another Kalman filter estimate of the Euler angles, is it valid > to use the direction cosine in the H matrix? > > - Given that the acceleration measurements come from accelerometers, > how much should I worry about accelerometer bias and drift(which does > exist) as well subtracting out gravity or will the Kalman filter > minimize its effect. > > Now on to the implementation. The Kalman filter time update equations > project the state and covariance from the aposteriori estimates from > the last time step: > > Xest_apri(k+1) = A(k)Xest_apost(k) > P_apri(k+1) = A(k)P_apost(k)A_T(k) + Q(k) > > and the Kalman filter measurement update equations update the > estimates from the apriori estimates of the last time step: > > K(k) = P_apri(k)H_T(k) Inv(H(k)P_apri(k)H_T(k) + R(k)) > Xest_apost(k) = Xest_apri(k) + K(k)(Z(k) - H(k)Xest_apri(k)) > P_apost(k) = (I - K(k)H(k)) P_apri(k) > > In my application, different sensor data arrive asynchronously at > different times. I however need the state estimates at a higher rate > synchronously to drive other parts of my control system. So some > questions I had on the implementations are: > > - My understanding from the above equations are that when a > measurement arrives, the time update equations need to be run to > project the apriori estimates for the current measurement time from > the last aposteriori estimate and then the measurement update > equations need to be run to get the new aposteriori estimate. When I > need estimates for my synchronous loop, only the time update equations > need to be run. So I run both time update and measurement update > equations when measurements arrive and only the time update equation > when a estimate is desired for my synchronous loop. Is this correct? > > - Given that what I stated in my last question is true, if no > measurement arrives between successive synchronous updates, would I > use the last aposteriori estimate from the last measurement update in > my time update equation to generate the second synchronous update? > That is, would I use the time update equation successively but with > the same aposteriori estimates for X and P. > > - I am a little confused about the measurement update equations. Given > that when a sensor update arrives, it only updates part of the > measurement vector z(k). For example, I have a sensor that updates the > x,y,z velocities in body frame? I don't have a new z position and > x,y,z acceleration update at this time. Do I still run the full > measurement update equation, even though only part of the measurement > vector is new? It seems counter intuitive to me since it seems you are > telling the filter that for part of the measurement vector there is > new data when there really isn't. There also doesn't seem a way to > break up the state and measurement vectors into smaller units and > separate filters so that this problem doesn't arise. Any comments? > > Thanks > > TIASee if this is helpful http://www.edu-observatory.org/gps/gps_books.html#Kalman
Reply by ●February 13, 20042004-02-13
Sam Wormley wrote:> > See if this is helpful > http://www.edu-observatory.org/gps/gps_books.html#KalmanGosh, Sam, did you need to quote the whole question to add that? Bob -- "Things should be described as simply as possible, but no simpler." A. Einstein
Reply by ●February 13, 20042004-02-13
"ta" <motorpg@yahoo.com> wrote in message news:d69d0bf.0402121543.47b8b746@posting.google.com...> I have some questions regarding a Kalman filter design and > implementation. I am new to Kalman filters so I apologize if my > questions are too basic. I also apologize for my fairly long post.I don't know enought about the Kalman filter part but it seems to me that you want to have a 6-degree of freedom model for which you want to predict position... isn't that right? the six dimensions are: x,y,z,rho,phi,theta At least it seems that for a body to be rotation insensitive would be unusual. But then, maybe it's a perfect sphere and not subject to currents / winds in the medium? Well, you did say it was an autonomous vehicle but not an autonomous *land* vehicle - so there could well be some simplifications like flat earth, no roll and pitch of any concern, etc. So, first I'd back up and forget about Kalman filters and figure out what your model for the body and the math for its state is and, *only then* decide why you need a Kalman filter in the first place. Usually you will have some sort of body model that includes forces due to weight, drag / lift, friction, propulsion and acceleration forces for moments of intertia, mass, etc. with possibly higher order effects included in each. I don't think a Kalman filter gets you out of needing to do this. Now, all of this gives you a deterministic model so you can predict rather well the position in the future given some set of forcing functions. And, carried to its extreme you would be able to predict position perfectly without any measurements at all as long as you know the forcing functions (including current / wind). But, let's say that now you get out into the real world and you have a notion of as you say: - z in world frame and - x,y,z velocities and accelerations in body frame. - an estimate of the attitude in Euler angles from other sensors which gives a direction cosine to convert from body to world frame. And you will convert everything into world frame for the model. Hopefully the accelerations include accelerations in rho, phi and theta - from which you can compute velocity or also you have velocities in rho, phi and theta. It appears the starting orientation is immaterial except for z because you only have velocities and accelerations. So the body could be heading north or south and we don't care. We assume the measurements aren't perfect - they are noisy. Then the starting point is somewhat unknown and the history is somewhat unknown. A Kalman filter might help you predict where the body will be in the future given the noisy history. I don't know how you do that without the model... I hope someone will let me know if I'm way off base or this is reasonable / helpful.... I hope to generate some discussion to get some insights of my own. Fred
Reply by ●February 13, 20042004-02-13
Kalman is normally used as a tracking filter used to predict future position based on pasted and present movement........well thats what I was taught at weapons school. Fred Marshall wrote:> "ta" <motorpg@yahoo.com> wrote in message > news:d69d0bf.0402121543.47b8b746@posting.google.com... >> I have some questions regarding a Kalman filter design and >> implementation. I am new to Kalman filters so I apologize if my >> questions are too basic. I also apologize for my fairly long post. > > I don't know enought about the Kalman filter part but it seems to me > that you want to have a 6-degree of freedom model for which you want > to predict position... isn't that right? > the six dimensions are: x,y,z,rho,phi,theta > > At least it seems that for a body to be rotation insensitive would be > unusual. But then, maybe it's a perfect sphere and not subject to > currents / winds in the medium? Well, you did say it was an > autonomous vehicle but not an autonomous *land* vehicle - so there > could well be some simplifications like flat earth, no roll and pitch > of any concern, etc. > > So, first I'd back up and forget about Kalman filters and figure out > what your model for the body and the math for its state is and, *only > then* decide why you need a Kalman filter in the first place. > > Usually you will have some sort of body model that includes forces > due to weight, drag / lift, friction, propulsion and acceleration > forces for moments of intertia, mass, etc. with possibly higher order > effects included in each. I don't think a Kalman filter gets you out > of needing to do this. > > Now, all of this gives you a deterministic model so you can predict > rather well the position in the future given some set of forcing > functions. And, carried to its extreme you would be able to predict > position perfectly without any measurements at all as long as you > know the forcing functions (including current / wind). > > But, let's say that now you get out into the real world and you have a > notion of as you say: > - z in world frame and > - x,y,z velocities and accelerations in body frame. > - an estimate of the attitude in Euler angles from other sensors > which gives a direction cosine to convert from body to world frame. > And you will convert everything into world frame for the model. > > Hopefully the accelerations include accelerations in rho, phi and > theta - from which you can compute velocity or also you have > velocities in rho, phi and theta. > It appears the starting orientation is immaterial except for z > because you only have velocities and accelerations. So the body > could be heading north or south and we don't care. > > We assume the measurements aren't perfect - they are noisy. Then the > starting point is somewhat unknown and the history is somewhat > unknown. A Kalman filter might help you predict where the body will > be in the future given the noisy history. I don't know how you do > that without the model... > > I hope someone will let me know if I'm way off base or this is > reasonable / helpful.... I hope to generate some discussion to get > some insights of my own. > > Fred
Reply by ●February 13, 20042004-02-13
Hmmm ..I learned Kalman filters from Rudy (Kalman) himself. Given that was 40 years ago, is that the latest art?? Phil Fred Marshall wrote:> "ta" <motorpg@yahoo.com> wrote in message > news:d69d0bf.0402121543.47b8b746@posting.google.com... > >>I have some questions regarding a Kalman filter design and >>implementation. I am new to Kalman filters so I apologize if my >>questions are too basic. I also apologize for my fairly long post. > > > I don't know enought about the Kalman filter part but it seems to me that > you want to have a 6-degree of freedom model for which you want to predict > position... isn't that right? > the six dimensions are: x,y,z,rho,phi,theta > > At least it seems that for a body to be rotation insensitive would be > unusual. But then, maybe it's a perfect sphere and not subject to currents > / winds in the medium? Well, you did say it was an autonomous vehicle but > not an autonomous *land* vehicle - so there could well be some > simplifications like flat earth, no roll and pitch of any concern, etc. > > So, first I'd back up and forget about Kalman filters and figure out what > your model for the body and the math for its state is and, *only then* > decide why you need a Kalman filter in the first place. > > Usually you will have some sort of body model that includes forces due to > weight, drag / lift, friction, propulsion and acceleration forces for > moments of intertia, mass, etc. with possibly higher order effects included > in each. I don't think a Kalman filter gets you out of needing to do this. > > Now, all of this gives you a deterministic model so you can predict rather > well the position in the future given some set of forcing functions. And, > carried to its extreme you would be able to predict position perfectly > without any measurements at all as long as you know the forcing functions > (including current / wind). > > But, let's say that now you get out into the real world and you have a > notion of as you say: > - z in world frame and > - x,y,z velocities and accelerations in body frame. > - an estimate of the attitude in Euler angles from other sensors which gives > a direction cosine to convert from body to world frame. > And you will convert everything into world frame for the model. > > Hopefully the accelerations include accelerations in rho, phi and theta - > from which you can compute velocity or also you have velocities in rho, phi > and theta. > It appears the starting orientation is immaterial except for z because you > only have velocities and accelerations. So the body could be heading north > or south and we don't care. > > We assume the measurements aren't perfect - they are noisy. Then the > starting point is somewhat unknown and the history is somewhat unknown. A > Kalman filter might help you predict where the body will be in the future > given the noisy history. I don't know how you do that without the model... > > I hope someone will let me know if I'm way off base or this is reasonable / > helpful.... I hope to generate some discussion to get some insights of my > own. > > Fred > >
Reply by ●February 14, 20042004-02-14
1. You should pick up a book on Inertial Navigation. These typically will have discussions and derivations on Kalman Filter inplementations. 2. In general, the position mesurements are, in fact, treated as measurement data for the filter. However, accelerometer and vehicle orientation (both from an IMU device) are usually treated as inputs to a Kalman Filter using as its model the "error" states. That is, the error model is derived from the Inertial Navigation mechanization equations and the effects of erroneous sensor data, etc. These error equations would then contain states to estimate sensor biases, etc. as well as the position and orientatation error states. Once the error states are estimated they are applied to the navigator in order to correct the solution. Harvey ta wrote:> I have some questions regarding a Kalman filter design and > implementation. I am new to Kalman filters so I apologize if my > questions are too basic. I also apologize for my fairly long post. > > My application is an autonomous vehicle. I need estimates of x,y,z > position and velocity in world frame. I have measurements from > multiple sensors giving me z in world frame and x,y,z velocities and > accelerations in body frame. I also have an estimate of the attitude > in Euler angles from other sensors which gives me a direction cosine > to convert from body to world frame. > > So given the Kalman state and measurement equations: > > x(k) = A(k-1)x(k-1) + w(k-1) > z(k) = H(k)x(k) + v(k) > > Here is my filter model, given that I am trying to initially use a > simple (not extended) Kalman filter. My x(k) is a 9x1 vector of the > positions, velocities and accelerations in world frame: i,e x(k) is > > [ Xp_wf ] > [ Yp_wf ] > [ Zp_wf ] > [ Xv_wf ] > [ Yv_wf ] > [ Zv_wf ] > [ Xa_wf ] > [ Ya_wf ] > [ Za_wf ] > > > Then A(k) follows from the kinematics as: > > [ 1 0 0 dt 0 0 0.5*dt*dt 0 0 ] > [ 0 1 0 0 dt 0 0 0.5*dt*dt 0 ] > [ 0 0 1 0 0 dt 0 0 0.5*dt*dt] > [ 0 0 0 1 0 0 dt 0 0 ] > [ 0 0 0 0 1 0 0 dt 0 ] > [ 0 0 0 0 0 1 0 0 dt ] > [ 0 0 0 0 0 0 1 0 0 ] > [ 0 0 0 0 0 0 0 1 0 ] > [ 0 0 0 0 0 0 0 0 1 ] > > > where dt is time step. > > My measurement vector z(k) is a 7x1 vector consisting of the z > position in world frame and the velocities and accelerations in body > frame i.e. z(k) is > > [ MZp_wf ] > [ MXv_bf ] > [ MYv_bf ] > [ MZv_bf ] > [ MXa_bf ] > [ MYa_bf ] > [ MZa_bf ] > > The H(k) matrix is fairly straightforward but is a little cumbersome > to list so I will just say that it converts the world frame velocities > and accelerations in the state vector to the body frame velocites and > acceleration in the measurement vector using the direction cosine > matrix. The Z position measurement is a 1:1 mapping to the Z position > in the state vector. > > > > > OK now on to some questions about the design: > > - Is this the best way to model the state and measurement, given what > I want an estimate of? I have seen examples where the accelerations > are not used in the state vector but as u(k) a driving function in the > state equation. I am not sure how a measurement(acceleration) can be > used in the state equation. > > - Given that the direction cosine matrix will in effect be derived > from another Kalman filter estimate of the Euler angles, is it valid > to use the direction cosine in the H matrix? > > - Given that the acceleration measurements come from accelerometers, > how much should I worry about accelerometer bias and drift(which does > exist) as well subtracting out gravity or will the Kalman filter > minimize its effect. > > > > > > Now on to the implementation. The Kalman filter time update equations > project the state and covariance from the aposteriori estimates from > the last time step: > > Xest_apri(k+1) = A(k)Xest_apost(k) > P_apri(k+1) = A(k)P_apost(k)A_T(k) + Q(k) > > and the Kalman filter measurement update equations update the > estimates from the apriori estimates of the last time step: > > K(k) = P_apri(k)H_T(k) Inv(H(k)P_apri(k)H_T(k) + R(k)) > Xest_apost(k) = Xest_apri(k) + K(k)(Z(k) - H(k)Xest_apri(k)) > P_apost(k) = (I - K(k)H(k)) P_apri(k) > > > In my application, different sensor data arrive asynchronously at > different times. I however need the state estimates at a higher rate > synchronously to drive other parts of my control system. So some > questions I had on the implementations are: > > - My understanding from the above equations are that when a > measurement arrives, the time update equations need to be run to > project the apriori estimates for the current measurement time from > the last aposteriori estimate and then the measurement update > equations need to be run to get the new aposteriori estimate. When I > need estimates for my synchronous loop, only the time update equations > need to be run. So I run both time update and measurement update > equations when measurements arrive and only the time update equation > when a estimate is desired for my synchronous loop. Is this correct? > > - Given that what I stated in my last question is true, if no > measurement arrives between successive synchronous updates, would I > use the last aposteriori estimate from the last measurement update in > my time update equation to generate the second synchronous update? > That is, would I use the time update equation successively but with > the same aposteriori estimates for X and P. > > - I am a little confused about the measurement update equations. Given > that when a sensor update arrives, it only updates part of the > measurement vector z(k). For example, I have a sensor that updates the > x,y,z velocities in body frame? I don't have a new z position and > x,y,z acceleration update at this time. Do I still run the full > measurement update equation, even though only part of the measurement > vector is new? It seems counter intuitive to me since it seems you are > telling the filter that for part of the measurement vector there is > new data when there really isn't. There also doesn't seem a way to > break up the state and measurement vectors into smaller units and > separate filters so that this problem doesn't arise. Any comments? > > > Thanks > > TIA
Reply by ●February 16, 20042004-02-16
Can you suggest a book that is not too heavy on the math? I am mostly looking for implementation examples. I have looked at enough derivations and would rather look at real world examples. Also I'm still looking for comments on the approach I describe. It seems fairly straightforward and I had hoped someone would comment on the filter model. TIA Harvey Gratt <hgratt@comcast.net> wrote in message news:<p7sXb.36419$_44.29993@attbi_s52>...> 1. You should pick up a book on Inertial Navigation. These typically > will have discussions and derivations on Kalman Filter inplementations. > > 2. In general, the position mesurements are, in fact, treated as > measurement data for the filter. However, accelerometer and vehicle > orientation (both from an IMU device) are usually treated as inputs to a > Kalman Filter using as its model the "error" states. That is, the error > model is derived from the Inertial Navigation mechanization equations > and the effects of erroneous sensor data, etc. These error equations > would then contain states to estimate sensor biases, etc. as well as the > position and orientatation error states. Once the error states are > estimated they are applied to the navigator in order to correct the > solution. > > Harvey >
Reply by ●February 17, 20042004-02-17
ta wrote:> Can you suggest a book that is not too heavy on the math? I am mostly > looking for implementation examples. I have looked at enough > derivations and would rather look at real world examples.Try R.M. Rogers, "Applied Mathematics in Integrated Navigation Systems", published by AIAA. It is all about using Kalman filters to combine navigation solutions from multiple sensors. Lars
Reply by ●February 18, 20042004-02-18
Try this URL http://ourworld.compuserve.com/homepages/PDJoseph/kalman.htm I bought the lessons from Peter Joseph and the seemed to be quite clear. Lars Erup <lerup@videotron.ca> wrote in message news:<60BYb.130590$gi7.2057990@weber.videotron.net>...> ta wrote: > > > Can you suggest a book that is not too heavy on the math? I am mostly > > looking for implementation examples. I have looked at enough > > derivations and would rather look at real world examples. > > Try R.M. Rogers, "Applied Mathematics in Integrated Navigation Systems", > published by AIAA. It is all about using Kalman filters to combine > navigation solutions from multiple sensors. > > Lars