1. M[-1|-1] = C
Initialize Covariance Matrix
representing the initial
estimate of the MSE of the
state estimate. The larger
this is, the greater credence
the filter gives
measurements
Linearize A and H about the state
A[n-1] = ∂a/∂s(n-1) such that s(n-1) = ŝ(n|n-1)
H[n] = ∂h(n)/∂s(n) such that s(n) = ŝ(n-1|n-1)
ŝ[-1|-1] = μ
Initialize State
Variables Based
on Past Data and
Physical
Parameters
Convert State
Variables to
Measured
Variables with
Measurement
Matrix to obtain
x[n]
Calculate Mean Square Error of
Prediction. This is the previous A times
the MSE of the estimate times A’ plus B
times the Covariance Matrix Q from the
State Model times B’
Compute Kalman Gain. This is a series of
matrix transformations that adjusts the
prediction MSE according to the ratio of the
predicted error and the state error plus a
weighted version of the spread of the
measured variables. This multiplies the
Difference Between the Predicted and
Actual Measurements
Initializations:
ŝ[-1|-1] = μ
M[-1|-1] = C
Prediction:
ŝ[n|n-1] = a(ŝ[n-1|n-1])
State Model:
s[n] = As[n-1]+Bu[n], u[n] WGN ~ (0,Q)
Linearization Of State Model:
A[n-1] = ∂a/∂s(n-1) such that s(n-1) = ŝ(n|n-1)
Measurement Model:
x[n] = Hs[n-1] + w[n], w[n] WGN ~(0,μ)
Linearization of Measurement Model:
H[n] = ∂h(n)/∂s(n) such that s(n) = ŝ(n-1)
Neither u[n] nor w[n] are Wide Sense Stationary
a(s) and h(s) are vector valued functions of the vector valued
state
N = Number of States (Approximate States to Ensure
Convergence TBD by Testing)
NEW STATE
Define A and H for
Current State
Multiply ŝ(n-1|n-1)
by A to obtain
ŝ[n|n-1]
FUNCTION CALL: ŝ(n-1|n-1)
For count = 1:NINIT
Run StateGen to Generate Ideal States and
Measurements to Test Filter Tuning
x[n] = Hs[n-1] + w[n], w[n] WGN ~(0,μ)
s[n] = As[n-1]+Bu[n], u[n] WGN ~ (0,Q)
INPUT FOR TEST PATH
FUNCTION CALL TO KALMAN ROUTINE: INPUTS:
Cn: Covariance matrix of w_n
Initial State ŝ[-1|-1]
Mean Square Error Of Estimate (Covariance Matrix Cs)
Q: Covariance Matrix of u_n
B: Matrix to Distribute Disturbances Among States
Matrix of N Measurements (MXN)
OFFLINE
AHEAD OF
TIME
OFFLINE
AHEAD OF
TIME
OFFLINE
AHEAD OF
TIME
3U KALMAN FILTER
FLOW INCLUDING TEST
PATH
OFFLINE AHEAD OF TIME FUNCTION
PARAMETERS ARE RESULT OF
TUNING AND ARE BLUE
Parse Input As
Matrix With
Measurements in
Columns (M X N)
GPS: 16b latitude 16b
Longitude
16b/axis Magnetic
Field
16b/axis Angular Velocity
Output Estimated
State Matrix With
States in Columns
16b/Value Buffers
Right Ascention
Tangential Velocity
Azimuth
Yaw
Pitch
Roll