![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](https://image.slidesharecdn.com/e0821206-19de-4294-84aa-76cd83e63229-150430163258-conversion-gate01/85/KalmanFlow_2nd_draft-1-320.jpg)
KalmanFlow_2nd_draft
- 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