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
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
Add image file

Polytechforum.com is a website by engineers for engineers. It is not affiliated with any of manufacturers or vendors discussed here. All logos and trade names are the property of their respective owners.