线性卡尔曼滤波和扩展卡尔曼滤波（KF，EKF）

线性$Kalman$滤波器$(KF)$

考虑如下状态空间模型描述的动态系统：

$$x_k=A{x}_{k-1}+B{u}_{k-1}+{\omega }_{k-1}$$

$$y_k=H{x}_k+v_k$$

式中，$k$为离散时间，系统在时刻$k$的状态向量为$x_k\in {\mathbb{R}}^n$；假设系统满足可观性要求，$y_k\in {\mathbb{R}}^n$为对应状态的观测向量；随机信号$w_k$和$v_k$分别表示过程噪声和观测噪声。

假设1：$w_k$和$v_k$是均值为零，方差阵各为$Q$和$R$的不相关白噪声,即$w_k\backsim N(0,Q)$，$v_k\backsim N(0,R)$，实际过程中$Q$和$R$可能随着每次迭代计算而变化，这里假设为常数。

假设2：初始状态$x_0$不相关于$w_k$和$v_k$，$E[x_0]={\mu }_0$，$E[(x_0-{\mu }_0)(x_0-{\mu }_0{)}^T]=P_0$

$Kalman$滤波器递推算法如下：

预测更新方程：$\{\begin{array}{l}{\bar{x}}_k=A{x}_{k-1}+B{u}_{k-1}\\ {\bar{P}}_k=A{P}_{k-1}{A}^T+Q\end{array}$

卡尔曼增益矩阵：$K={\bar{P}}_k{H}^T(H{\bar{P}}_k{H}^T+R{)}^{-1}$

测量更新方程：$\{\begin{array}{l}{x}_k={\bar{x}}_k+K(y_k-H{\bar{x}}_k)\\ P_k=(I-KH){\bar{P}}_k\end{array}$

测量误差对滤波结果的影响：

当$R$趋于0时，${\lim }_{R\to 0}K=H^{-1}$，测量噪声$v=0$,系统表现为完全取测量值作为状态的后验估计，而系统的先验状态估计被完全抛弃；当${\bar{P}}_k=0$时，$Q=0$，此时系统完全抛弃测量值，取先验估计值。

过程噪声$Q$对滤波效果的影响：

$Q$为对模型的信任度。$Q$越大滤波后的曲线跟测量曲线跟的越紧密，滤波后噪声越大；$Q>1$ 时，滤波效果差，值越大效果越差，$Q$值越小滤波效果越明显。

协方差矩阵$P$对滤波效果的影响：

$P$决定初始增益的大小，影响初始时刻的收敛效果，在整体参数选取合适的时候，初始时刻滤波收敛效果影响不明显。

测量噪声$R$对滤波效果的影响：

$R$决定稳态噪声。$R$越小初始增益越大收敛越快，但是稳态容易引入噪声；$R$越大对噪声越不敏感，即滤波后的数据跳动越小，但$R$越大卡尔曼滤波输出收敛得越慢。

状态估计流程图：

误差协方差运算流程图：

扩展$Kalman$滤波器$(EKF)$

普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得目标的动态估计，适应于过程和测量都属于线性系统， 且噪声符合高斯分布的系统。 但是实际上很多系统都存在一定的非线性,表现在过程方程 （状态方程）是非线性的，或者观测与状态之间的关系（测量方程）是非线性的。这种情况下就不能使用一般的卡尔曼滤波。解决的方法是将非线性关系进行线性近似，将其转化成线性问题。对于非线性问题线性化常用的两大途径∶

（1）将非线性环节线性化，对高阶项采用忽略或逼近措施：扩展卡尔曼滤波$(EKF)$；

（2）用采样方法近似非线性分布：无迹卡尔曼滤波$(UKF)$。

$EKF$算法是一种近似方法，它将非线性模型在状态估计值附近作泰勒级数展开，并做一阶截断，用得到的一阶近似项作为原状态方程和测量方程近似表达形式，从而实现线性化同时假定线性化后的状态依然服从高斯分布，然后对线性化后的系统采用标准卡尔曼滤波获得状态估计。采用局部线性化技术，能得到问题局部最优解，但它能否收敛于全局最优解，取决于函数的非线性强度以及展开点的选择。

假定非线性状态方程和测量方程如下：

$$X_{k+1}=f(X_k)+W_k$$

$$Y_k=h(X_k)+V_k$$

在最近一次状态估计的时刻对以上两式进行线性化处理，得到$f和h$ 的雅可比矩阵：

$$F=\frac{\partial f}{\partial X}=\left[\begin{array}{cccc}\frac{\partial f_1}{\partial X_1}& \frac{\partial f_1}{\partial X_2}& \cdots & \frac{\partial f_1}{\partial X_n}\\ \frac{\partial f_2}{\partial X_1}& \frac{\partial f_2}{\partial X_2}& \cdots & \frac{\partial f_2}{\partial X_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial f_n}{\partial X_1}& \frac{\partial f_n}{\partial X_2}& \cdots & \frac{\partial f_n}{\partial X_n}\end{array}\right]$$

$$H=\frac{\partial h}{\partial X}=\left[\begin{array}{cccc}\frac{\partial h_1}{\partial X_1}& \frac{\partial h_1}{\partial X_2}& \cdots & \frac{\partial h_1}{\partial X_n}\\ \frac{\partial h_2}{\partial X_1}& \frac{\partial h_2}{\partial X_2}& \cdots & \frac{\partial h_2}{\partial X_n}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{\partial h_n}{\partial X_1}& \frac{\partial h_n}{\partial X_2}& \cdots & \frac{\partial h_n}{\partial X_n}\end{array}\right]$$

扩展卡尔曼滤波器算法：

预测更新方程：$\{\begin{array}{l}{\bar{x}}_k=f(x_k)\\ {\bar{P}}_k=F{P}_{k-1}{F}^T+Q\end{array}$

卡尔曼增益矩阵：$K={\bar{P}}_k{H}^T(H{\bar{P}}_k{H}^T+R{)}^{-1}$

测量更新方程：$\{\begin{array}{l}{x}_k={\bar{x}}_k+K(y_k-H{\bar{x}}_k)\\ P_k=(I-KH){\bar{P}}_k\end{array}$