Kalman filter for inertial navigation

Forgive me, but I am new to Kalman filters. I am having problems
designing a Kalman filter for inertial position and velocity
Given the state and measurement Kalman equations:
X(t+1) = A(t)X(t) + w(t)
Y(t) = H(t)X(t) + v(t)
What I want is position and velocity estimates for x,y,z,roll,pitch
and yaw. I have measurements for roll,pitch and yaw and their rates so
the state and measurement equations are fairly obvious given the state
vector being position and velocity. For x,y and z axis, I want the
estimate for the inertial position and velocities. The measurements I
have are x,y in body frame and z in inertial frame, the 3 velocities
and accelerations in body frame with the gravity vector added in for
z. Given that I have the roll, pitch and yaw, I have the direction
cosine available to convert from body to inertial(world) frame. The
problem I am having is that given that X(t) is in inertial frame and
Y(t) is in body frame I am having trouble coming up with a H(t)
matrix. Also I am having trouble subtracting out the gravity vector in
the context of the measurement equation.
There must be some examples of something similar that is done in
aircraft navigation. If someone can point me to a simple description
of this, it would be helpful.
- bijtaj
Reply to
Loading thread data ...

PolyTech Forum website is not affiliated with any of the manufacturers or service providers discussed here. All logos and trade names are the property of their respective owners.