# Kalman filter for inertial navigation

• posted
Hi,
Forgive me, but I am new to Kalman filters. I am having problems
designing a Kalman filter for inertial position and velocity
estimation.
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