扩展卡尔曼滤波器

卡尔曼滤波器是最优的线性滤波器，在非线性系统中，没有办法用线性的状态空间方程表达，只能以如下形式表达：
$$\begin{array}{rl}{X}_k& =f(X_{k-1},U_{k-1},W_{k-1})\\ Z_k& =h(X_k,V_k)\end{array}$$
$f、h$为非线性函数，然而，正态分布的随机变量通过非线性系统后就不再是正态的了，故我们需要对其线性化，利用泰勒展开。
$$f(x)=f(x_0)+\frac{\partial f}{\partial x}(x-x_0)$$
看得出来，若我们想要线性化一个系统，我们需要找到一个真实点，在其附近进行线性化。但系统有误差，我们没法知道真实点，则我们可以对于$f(x_k)$，在上一次的后验估计值${\hat{x}}_{k-1}$——即最佳估计值附近线性化：
$$X_k\approx f({\hat{X}}_{k-1},U_{k-1})+\underset{\frac{\partial f}{\partial X}{|}_{X={\hat{X}}_{k-1}}}{\underbrace{J_{k-1}}}(X_{k-1}-{\hat{X}}_{k-1})+W_{k-1}$$
对于观测函数：
$$Z_k\approx h({\hat{X}}_k^{-})+\underset{\frac{\partial h}{\partial X}{|}_{X={\hat{X}}_k^{-}}}{\underbrace{H_k}}(X_k-{\hat{X}}_k^{-})+V_k$$
先验估计（使用非线性函数直接观测）：
$${\hat{X}}_k^{-}=f({\hat{X}}_{k-1},U_k-1)$$
先验误差协方差：

首先计算误差：
$$\begin{array}{rlrl}{e}_k^{-}& =X_k-{\hat{X}}_k^{-}& & ①\\ X_k& \approx f({\hat{X}}_{k-1},U_{k-1})+J_{k-1}(X_{k-1}-{\hat{X}}_{k-1})+W_{k-1}& & ②\\ {\hat{X}}_k^{-}& =f({\hat{X}}_{k-1},U_{k-1})& & ③\\ \Rightarrow e_k^{-}& \approx J_{k-1}{e}_{k-1}+W_{k-1}& & \end{array}$$
协方差传播：
$$\begin{array}{rl}{P}_k^{-}& =E[e_k^{-}{e}_k^{-T}]\\ & =E[(J_{k-1}{e}_{k-1}+W_{k-1})(J_{k-1}{e}_{k-1}+W_{k-1}{)}^T]\\ & =J_{k-1}E[e_{k-1}{e}_{k-1}^T]J_{k-1}^T+E[W_{k-1}{W}_{k-1}^T]\\ & =J_{k-1}{P}_{k-1}{J}_{k-1}^T+Q_{k-1}\end{array}$$
后验误差协方差：

后验估计误差：
$$\begin{array}{rl}{e}_k& =X_k-{\hat{X}}_k\\ & =X_k-{\hat{X}}_k^{-}-K_k(Z_k-h({\hat{X}}_k^{-}))\\ & \approx e_k^{-}-K_k{H}_k{e}_K^{-}-K_k{V}_k\\ & \approx (I-K_k{H}_k)e_k^{-}-K_k{V}_k\end{array}$$

则后验误差协方差：
$$\begin{array}{rl}{P}_k& =E[e_k{e}_k^T]\\ & =E[((I-K_k{H}_k)e_k^{-}-K_k{V}_k)((I-K_k{H}_k)e_k^{-}-K_k{V}_k{)}^T]\\ & =E[((I-K_k{H}_k)e_k^{-}-K_k{V}_k)(e_k^{-T}(I-H_k^T{K}_k^T)-K_k^T{V}_k^T)]\\ & =E[(I-K_k{H}_k)e_k^{-}{e}_k^{-T}(I-H_k^T{K}_k^T)-(I-K_k{H}_k)e_K^{-}{K}_k^T{V}_k^T-K_k{V}_k{e}_k^{-T}(I-H_k^T{K}_k^T)+K_k{V}_k{V}_k^T{K}_k^T]\\ & =E[(I-K_k{H}_k)e_k^{-}{e}_k^{-T}(I-H_k^T{K}_k^T)]-E[(I-K_k{H}_k)e_K^{-}{K}_k^T{V}_k^T]-E[K_k{V}_k{e}_k^{-T}(I-H_k^T{K}_k^T)]+E[K_k{V}_k{V}_k^T{K}_k^T]\\ & =\underset{\text{协方差更新的Joseph形式}}{\underbrace{(I-K_k{H}_k)P_k^{-}(I-H_k^T{K}_k^T)+K_k{R}_k{K}_k^T}}\\ & =P_k^{-}-K_k{H}_k{P}_k^{-}-P_k^{-}{H}_k^T{K}_k^T+K_k(H{P}_k^{-}{H}^T+R)K_k^T\end{array}$$
求解最优增益：

为了最小化$P_k$的迹，对$K_k$求偏导并令其为0：
$$\begin{array}{r}\frac{\partial tr(P_k)}{\partial K_k}=-2P_k^{-}{H}_k^T+2K_k(H_k{P}_k^{-}{H}^T+R)=0\end{array}$$
则最优卡尔曼增益为：
$$K_k=P_k^{-}{H}_k^T(H{P}_k^{-}HT+R{)}^{-1}$$
状态更新：

利用最优增益更新状态：
$${\hat{X}}_k={\hat{X}}_k^{-}+K_k[Z_k-h({\hat{X}}_h^{-})]$$
若$K_k$较大，更信任测量值，测量噪声小

若$K_k$较小，更信任预测值，预测误差小

协方差更新：

将最优增益带入$P_k$，可得：
$$\begin{array}{rl}{P}_k& =(I-K_k{H}_k)P_k^{-}\end{array}$$

完整流程

初始化：

$$\begin{array}{rl}{\hat{X}}_0& =E[X_0]\\ P_0& =Cov(X_0)\end{array}$$

预测阶段：

状态雅可比矩阵：
$$J_{k-1}=\frac{\partial f}{\partial X}{|}_{X={\hat{X}}_{k-1}}$$
状态预测（先验估计）：
$${\hat{X}}_k^{-}=f({\hat{X}}_{k-1}+U_{k-1})$$

误差协方差预测（先验误差协方差）：
$$P_k^{-}=J_{k-1}{P}_{k-1}{J}_{k-1}^T+Q_{k-1}$$

更新阶段：

观测雅可比矩阵：
$$H_k=\frac{\partial h}{\partial X}{|}_{X={\hat{X}}_k^{-}}$$

卡尔曼增益：
​ $$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$

状态更新（后验估计）：
​ $${\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_k-h({\hat{X}}_k^{-}))$$

误差协方差更新：
​ $$P_k=(I-K_{k}H)P_k^{-}$$
或$\text{Joseph}$形式：
$$P_k=(I-K_{k}H)P_k^{-}(I-K_{k}H{)}^T+K_{k}R{K}_k^T.$$