一、五个方程的推导

1 Introduction&THE BACKGROUND BREAK

www.kalmanfilter.net/default.asp…

对于air-plane state有九个，state extrapolation equation如下

\begin{cases}
x_{n} = x_{n-1} + \dot{x}_{n-1} \Delta t+ \frac{1}{2}\ddot{x}_{n-1} \Delta t^{2}\\
y_{n} = y_{n-1} + \dot{y}_{n-1} \Delta t+ \frac{1}{2}\ddot{y}_{n-1} \Delta t^{2}\\
z_{n} = z_{n-1} + \dot{z}_{n-1} \Delta t+ \frac{1}{2}\ddot{z}_{n-1} \Delta t^{2}\\
\dot{x}_{n} = \dot{x}_{n-1} + \ddot{x}_{n-1} \Delta t\\
\dot{y}_{n} = \dot{y}_{n-1} + \ddot{y}_{n-1} \Delta t\\
\dot{z}_{n} = \dot{z}_{n-1} + \ddot{z}_{n-1} \Delta t\\
\ddot{x}_{n} = \ddot{x}_{n-1}\\
\ddot{y}_{n} = \ddot{y}_{n-1}\\
\ddot{z}_{n} = \ddot{z}_{n-1}\\
\end{cases}

我们用matric来表示方程，好处如下:

First, it is very exhausting to write all these equations, representation in matrix notation is much shorter and elegant.
Second, the computers are extremely efficient in matrix calculations. Implementing the Kalman Filter in matrix form yields faster computation run time.

THE BACKGROUND BREAK:

Matrix operations
Covariance and covariance matrix
Expectation algebra

A geometric interpretation of the covariance matrix

www.visiondummy.com/2014/04/geo…

协方差矩阵定义了数据的形状；对角线是方差，非对角线是协方差

对于向量:（对角线是方差，非对角线是协方差）

2 State Extrapolation Equation

www.kalmanfilter.net/stateextrap…

意义：Using the state extrapolation equation, we can predict the next system state, based on the knowledge of the current state. It extrapolates state vector from the present (time step n ) to the future (time step n+1 ).

3 Modeling Linear Dynamic systems

4 Covariance Extrapolation equation

Covariance extrapolation Equation：下一个状态估计不确定度的预测

动态模型的不确定度被称为过程噪声Process Noise，需要被加进Covariance extrapolation Equation中

一维卡尔曼滤波中， $q_{n}$ 代表process noise，太小会产生lag error，太大会使卡尔曼增益变大使得估计更看重measment

多维卡尔曼滤波中，Covariance Extrapolation equation如下：P是协方差矩阵，包含了方差和协方差，描述了状态的不确定度

构造过程噪声矩阵Q：

DISCRETE NOISE MODEL：

CONTINUOUS NOISE MODEL：

如何选择噪声矩阵Q模型：

①首先要选择正确的process noise variance $\sigma$

(上面两个例子中process noise variance是 $\sigma_{a}$ ，是constant velocity motion model中0加速度的方差，表示过程噪声)

②然后 When Δt is very small you can use the discrete noise model, when Δt is large it is better to use the continuous noise model. I recommend trying both models and check which one performs better with your Kalman Filter

5 Measurement Equation

在许多情况下，测量值不是所需的系统状态,系统状态是不可测的。例如，数字电子温度计测量电流，而系统状态是温度。需要将系统状态（输入）转换为测量（输出）。

所以The purpose of the observation matrix H is to convert system state into outputs using linear transformations.

H选择的三个Example:

★6 中期总结

卡尔曼滤波器基于五个方程

Two prediction equations:

State Extrapolation Equation - prediction or estimation of the future state, based on the known present estimation.

Covariance Extrapolation Equation - the measure of uncertainty in our prediction.

Two update equations:

State Update Equation - estimation of the current state, based on the known past estimation and present measurement.

Covariance Update Equation - the measure of uncertainty in our estimation.

Kalman Gain Equation – required for computation of the update equations. The Kalman Gain is actually a "weighting" parameter for the measurement and the past estimations. It defines the weight of the past estimation and the weight of the measurement in estimating the current state.

一 PREDICTION EQUATIONS

1 STATE EXTRAPOLATION EQUATION

2 Covariance Extrapolation Equation

二 AUXILIARY EQUATIONS

1 MEASUREMENT EQUATION

★2 COVARIANCE EQUATIONS

3 MEASUREMENT UNCERTAINTY

4 PROCESS NOISE UNCERTAINTY

5 ESTIMATION UNCERTAINTY

7 THE KALMAN GAIN

通过打开covariance update equation来推导

具体见：www.kalmanfilter.net/kalmanGain.…

★【关键点】：

kalman filter's goal is to minimize the esitimate variance, and trace(P_{n×n}) 就表示the esitimate variance,so our goal is to minimize the trace(P_{n×n})

所以对 trace(P_{n×n}) 关于Kn求导并令成0，便可推导出Kn的最优更新式

8 STATE UPDATE EQUATION

The innovation contains the new information.

9 COVARIANCE UPDATE EQUATION

推导使用的方程:

具体推导过程:www.kalmanfilter.net/covUpdate.h…

SIMPLIFIED COVARIANCE UPDATE EQUATION：

推导过程:www.kalmanfilter.net/simpCovUpda…

缺点：even the smallest error in computing the Kalman Gain (due to round off) can lead to huge computation errors. The subtraction (I−KnH) can lead to nonsymmetric matrixes results due to the floating-point errors. This equation is numerically unstable!

二、总结

Once initialized, the Kalman Filter shall predict the system state at the next time step. As well, the Kalman Filter provides the uncertainty of the prediction.

Once the measurement is received, the Kalman Filter updates (or corrects the prediction) and the uncertainty of the current state. As well, the Kalman Filter predicts the next states, and so on.

kalman filter's goal is to minimize the esitimate variance, and trace(P_{n×n}) 就表示the esitimate variance,so our goal is to minimize the trace(P_{n×n})