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
>

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

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

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

"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

Sam Wormley wrote:

> 
> See if this is helpful
>   http://www.edu-observatory.org/gps/gps_books.html#Kalman

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

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

See if this is helpful
  http://www.edu-observatory.org/gps/gps_books.html#Kalman