
GPS-aided INS Solution for
OpenPilot
Dale E. Schinstock
Kansas State University
NOMENCLATURE
SYSTEM MODEL:
),,( wuxf
- nonlinear state equations
)(xh
- nonlinear measurement equations
F - system matrix from linearization
G
- input matrix from linearization
H
- measurement matrix from linearization
x
- state vector
T
zyx
PPPP
- position vector in the NED (North-
East-Down) Earth fixed frame
T
zyx
VVVV
- velocity vector in the NED earth
fixed frame
T
qqqqq
3210
- attitude vector as a unit
quaternion
T
zyx
bbbb
- rate gyro bias vector
- Earth’s gravitational acceleration
mm
au
- input vector
T
zyx
aaaa
- true acceleration vector in the body
fixed frame
T
zyx
- true rotational rates vector in the
body fixed frame
a
w
- acceleration sensor noise vector
w
- angular rate sensor noise vector
b
w
- noise vector for bias random walks
T
T
b
T
a
T
wwww
- process noise vector
T
v
- measurement noise vector
T
mzmymxm
aaaa
- measured acceleration vector in
the body fixed frame
T
mzmymxm
- measured rotational rate
vector in the body fixed frame
eb
R - rotation matrix rotating vectors in the body fixed
frame to the earth fixed frame
Ω - matrix responsible for converting angular rates in
the body fixed frame to quaternion rates
b
A
- barometric altitude measurement
e
B
- magnetic vector in Earth frame
b
B
- magnetic vector measurement in body frame
- measurement vector
y
- prediction of measurements from the state vector
EXTENDED KALMAN FILTER IMPLEMENTATION:
k
- discrete time version of a vector
Φ - state transition matrix
Γ - discrete time input matrix
- period of prediction step
),,,,,,,,(
222222222
zyxzyxzyx
bbbaaa
diag
Q
-
plant/disturbance noise covariance matrix
k
P - state estimate error covariance matrix
- Kalman gain matrix
),,,,,,,,,(
2222222222
AltBBBVVVPPP
zyxzyxzyx
diag
R -
measurement noise covariance matrix
SYSTEM MODEL
The dynamic system model developed here is a
kinematic model for a six DOF rigid body with position
and velocity represented in an inertial coordinate frame
(Earth fixed) and angular velocity and acceleration in a
body fixed frame. Because it is a kinematic model, it is
applicable to any vehicle, independent of the specific
dynamics of that vehicle. In addition to the dynamic
state variables, the state vector also includes bias states
for sensors, which are modeled dynamically as simple
random walks.
The development of the system model will make use of
two matrices that are stated here without derivation. The
first of these matrices is the rotation matrix as a function
of the unit quaternion.
3,32,31,3
3,22,21,2
3,12,11,1
)(
RRR
RRR
RRR
q
be
R
(1)
where
2
3
2
2
2
1
2
01,1
qqqqR
)(2
30212,1
qqqqR
)(2
20313,1
qqqqR
)(2
30211,2
qqqqR
2
3
2
2
2
1
2
02,2
qqqqR
)(2
10323,2
qqqqR
)(2
20311,3
qqqqR
)(2
10322,3
qqqqR
2
3
2
2
2
1
2
03,3
qqqqR
It is worthwhile to note that the inverse of the rotation
matrix is its transpose,
T
beebbe
RRR
1
. The second
of these matrices is used in the “strapdown” equation,
2/)(
qq Ω , to relate the body axis angular velocity to
the unit quaternion rates.