标准卡尔曼滤波(KF)和扩展卡尔曼滤波(EKF)介绍及推导

一、标准卡尔曼滤波介绍及推导

卡尔曼滤波本质是最小均方差估计，通过构造真实值与估计值的误差协方差矩阵，使得误差最小，从而进行最优估计。
标准卡尔曼滤波适用于线性系统。

1、标准卡尔曼滤波方程

首先构建系统的离散方程，如下：

(1)系统状态方程
$X(k)=AX(k-1)+BU(k)+W(k)$
(2)系统观测方程：
$Z(k)=HX(k)+V(k)$
其中：
X(k)：是k时刻的系统状态；
U(k)：是k时刻对系统的控制量；
A和B：是系统参数，对于多模型系统，他们为矩阵；
Z(k)：是k时刻的测量值；
H：是测量系统的参数，对于多测量系统，H为矩阵。
W(k)和V(k)：分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，协方差分别是Q，R（这里假设他们不随系统状态变化而变化）。

卡尔曼滤波方程的五个公式如下：

(1) $X(k|k-1)=AX(k-1|k-1)+BU(k)$

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态估计的最优结果，U(k)为某刻状态的控制量，如果没有控制量，它可以为0。

(2) $P(k|k-1)=AP(k-1|k-1)A^T+Q$

式(2)中，P(k|k-1)是X(k|k-1)对应的协方差，P(k-1|k-1)是X(k-1|k-1)对应的协方差，$A^T$表示A的转置矩阵，Q是过程噪声的协方差。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。
前两个方程为预测过程，后三个方程为校正过程。

(3) $Kg(k)=\frac{P(k|k-1)H^T}{(HP(k|k-1)H^T+R)}$

Kg为卡尔曼增益(Kalman Gain)。

(4) $X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))$

某刻状态(k)的最优化估算值X(k|k)。

(5) $P(k|k)=(I-Kg(k)H)P(k|k-1)$

更新k状态下X(k|k)的协方差。

更新流程图如下图所示：

2、标准卡尔曼滤波方程推导

k时刻的真实值与最优估计值之间的误差为：
$e(k)=X(k)-X(k|k)$
而最优估计值x(k|k)为：
$X(k|k)=X(k|k-1)+Kg(k)\hat{Z}(k)$
=$X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))$
=$X(k|k-1)+Kg(k)(HX(k)+V(k)-HX(k|k-1))$
其中$\hat{Z}(k)$为测量余差,即真实观测值相对于预测值的偏差。最优估计为预测量+测量余差的kg倍。
通过以上两个公式可得：
$e(k)=X(k)-X(k|k)$
=$X(k)-X(k|k-1)-Kg(k)HX(k)-KgV(k)+KgHX(k|k-1)$
=$(1-Kg(k)H)X(k)-(1-Kg(k)H)X(k|k-1)-KgV(k)$
=$(1-Kg(k)H)(X(k)-X(k|k-1))-Kg(k)V(k)$
误差的协方差矩阵为：
$P(k)=E(e(k)e(k{)}^T)$=$E\left\{[(1-Kg(k)H)(X(k)-X(k|k-1))-KgV(k)][(1-Kg(k)H)(X(k)-X(k|k-1))-KgV(k){]}^T\right\}$
=$(1-Kg(k)H)E[(X(k)-X(k|k-1))(X(k)-X(k|k-1){)}^T](1-Kg(k)H{)}^T+Kg(k)E[V(k)V(k{)}^T]Kg(k{)}^T$…(1)
其中：
$P(k|k-1)=E[(X(k)-X(k|k-1))(X(k)-X(k|k-1){)}^T]$为真实值与预测值之间的协方差。
$R=E[V(k)V(k{)}^T]$
将上述两式带入方程(1)得：
$P(k)$=$(1-Kg(k)H)P(k|k-1)(1-Kg(k)H{)}^T+Kg(k)RKg(k{)}^T$
=$P(k|k-1)-2Kg(k)HP(k|k-1)+Kg(k)(HP(k|k-1)H^T+R)Kg(k{)}^T$…(2)
需要求得Kg使得协方差P(k)最小，两边对Kg求导。
$\frac{d(P(k))}{d(Kg(k))}$= d { − 2 K g ( k ) H P ( k ∣ k − 1 ) + K g ( k ) ( H P ( k ∣ k − 1 ) H T + R ) K g ( k ) T } d ( K g ( k ) ) \frac{d\left \{-2Kg(k)HP(k|k-1)+Kg(k)(HP(k|k-1)H^{T}+R)Kg(k)^{T}\right \}}{d(Kg(k))} d(Kg(k))d{−2Kg(k)HP(k∣k−1)+Kg(k)(HP(k∣k−1)HT+R)Kg(k)T}​
= d { − 2 K g ( k ) H P ( k ∣ k − 1 ) } d ( K g ( k ) ) + d { K g ( k ) ( H P ( k ∣ k − 1 ) H T + R ) K g ( k ) T } d ( K g ( k ) ) \frac{d\left \{-2Kg(k)HP(k|k-1)\right \}}{d(Kg(k))}+\frac{d\left \{Kg(k)(HP(k|k-1)H^{T}+R)Kg(k)^{T}\right \}}{d(Kg(k))} d(Kg(k))d{−2Kg(k)HP(k∣k−1)}​+d(Kg(k))d{Kg(k)(HP(k∣k−1)HT+R)Kg(k)T}​
=$-2(HP(k|k-1){)}^T+2Kg(k)(HP(k|k-1)H^T+R)$
令其等于0，则得到：
$Kg(k)$=$\frac{P(k|k-1)H^T}{HP(k|k-1)H^T+R}$ 卡尔曼增益
$P^{\prime }(k)=E[(X(k)-X(k|k-1))(X(k)-X(k|k-1){)}^T]$
=$E\left\{[AX(k-1)+BU(k)+W(k)-A\hat{X}(k-1)-BU(k)][AX(k-1)+BU(k)+W(k)-A\hat{X}(k-1)-BU(k){]}^T\right\}$
=$E\left\{[AX(k-1)+W(k)-A\hat{X}(k-1)][AX(k-1)+W(k)-A\hat{X}(k-1){]}^T\right\}$
=$E\left\{[A(X(k-1)-\hat{X}(k-1))+W(k)][A(X(k-1)-\hat{X}(k-1))+W(k){]}^T\right\}$
=$E[(Ae(k-1))(Ae(k-1){)}^T]$+$E[W(k-1)W^T(k-1)]$
=$AP(k-1)A^T+Q$
至于$P(k|k)$，
$P(k|k)$=$P(k|k-1)-Kg(k)HP(k|k-1)-P(k|k-1)H^{T}Kg(k{)}^T+Kg(k)(H{P}^{\prime }(k)H^T+R)Kg(k{)}^T$
将kg带入则，
$P(k|k)$=$(I-Kg(k)H)P^{\prime }(k)$

二、扩展卡尔曼滤波介绍及推导

1、扩展卡尔曼滤波介绍

扩展卡尔曼滤波中心思想就是将非线性系统线性化之后再做KF处理。具体就是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架再对信号作处理，是一种次优滤波。
假设离散系统状态空间模型为：
(1)系统状态方程
$X(k)=f(X(k-1))+\Gamma (k-1)W(k-1)$
(2)观测方程
$Z(k)=h(X(k))+V(k)$
其中：
X(k)：是k时刻的系统状态；
f(X(k-1))、h(X(k))：非线性的；
Z(k)：是k时刻的测量值；
W(k)和V(k)：分别表示过程和测量的噪声，协方差分别是Q，R（这里假设他们随系统状态变化而变化）。*。

则扩展卡尔曼滤波的五个方程如下：
(1)$X(k|k-1)=f(X(k-1|k-1))$

(2)$P(k|k-1)=FP(k-1|k-1)F^T+\Gamma (k-1)Q(k)\Gamma (k-1)$
其中，F(k|k-1)为雅可比矩阵。
$F(k|k-1)=J(f(X(k-1|k-1)))=\frac{\partial f(X(k-1|k-1))}{\partial X(k-1|k-1)}$

(3)$Kg(k)=\frac{P(k|k-1)H(k{)}^T}{H(k)P(k|k-1)H(k{)}^T+R(k)}$
其中，H(k)为雅可比矩阵，h(X(k)在X(k)'处的导数。
$H(k)=J(h(X(k)))=\frac{\partial h(X(k))}{\partial {X(k)}^{\prime }}$

(4)$X(k|k)=X(k|k-1)+Kg(k)(Z(k)-h(X(k|k-1)))$

(5)$P(k|k)=(I-Kg(k)H)P(k|k-1)$

2、扩展卡尔曼滤波推导

具体推导过程可参考文章：
https://blog.youkuaiyun.com/weixin_42647783/article/details/89054641
无非就是将非线性方程做泰勒一阶展开，由此会得到F及H（均为雅可比矩阵），基于泰勒展开公式和标准卡尔曼滤波方程，就可直接写出扩展卡尔曼滤波方程的整个推导过程。