卡尔曼滤波器理论基础

强推DR_CAN的视频教学,讲的很清楚，而且例子举得也很通俗易懂。 [点击这里跳转](https://www.bilibili.com/video/BV1ez4y1X7eR/?spm_id_from=333.788&vd_source=c2b3fa1e2440ee7e7443aca0df4fb0bb)

### 1.本质：递归算法

引入一个情景，有一个人拿一个尺子去测量一个硬币的直径。共测量k次。这里每次的测量都不可避免的出现误差。

那我们怎么去估计这个硬币的真实直径呢？ 最直观简单的方式就是去计算k次测量值的平均值。
$$
记录第k次的测量结果为z_k。记录这个估计值为\hat{x}_k \\
\begin{aligned}
\hat{x}_k&=\frac{1}{k}(z_1+z_2+ \cdots z_k) \\
&=\frac{1}{k}(z_1+z_2+ \cdots z_{k-1})+\frac{1}{k}z_k \\
&=\frac{1}{k}\frac{k-1}{k-1}(z_1+z_2+ \cdots z_{k-1})+\frac{1}{k}z_k\\
&=\frac{k-1}{k}\hat{x}_{k-1}+\frac{1}{k}z_k \\
&=\hat{x}_{k-1}-\frac{1}{k}\hat{x}_{k-1}+\frac{1}{k}z_k
\end{aligned}\\
经过这一系列化简，最终可以得到如下的式子\\
\hat{x}_k=\hat{x}_{k-1}+\frac{1}{k}(z_k-x_{k-1})\\
由此获得一个结论，随着k的不断增加，\frac{1}{k}不断减少，\hat{x}_k越趋近于\hat{x}_{k-1}。即随着测量次数增加，测量结果就不在重要了。同理测量次数越少，测量结果的重要性越大。
$$
整理以上式子，将1/k等效于K_k。就有如下结论
$$
\hat{x}_k=\hat{x}_{k-1}+K_k(z_k-x_{k-1}) \\
K_k我们称为卡尔曼增益或卡尔曼因数\\
即：当前估计值=上一次的估计值+系数*（当前测量值-上一次估计值）
$$
从式子中我们知道，每一次的估计值都与上一次的估计值有关，这就是一种递归的思想。

接下来我们引进两个值，分布是估计误差e_EST，和测量误差e_MEA。关于K_k存在下列关系式
$$
K_k=\frac{e_{ESTk-1}}{e_{ESTk-1}+e_{MEAk}} \\
推导过程暂略，后续将给出详细证明 \\\\
我们接下来讨论在k时刻: \\
1.e_{ESTk-1}>>e_{MEAk}，即K_k\approx1,得出\hat{x}_k=z_k，可以理解为估计误差过大，我们就更倾向于相信我们的测量结果\\
2.e_{ESTk-1}<<e_{MEAk}，即K_k\approx0,得出\hat{x}_k=\hat{x}_{k-1}，可以理解为测量误差过大，我们就更倾向于相信我们的估计结果
$$

卡尔曼滤波器的简单应用过程如下
$$
\begin{aligned}
&第一步：计算K_k=\frac{e_{ESTk-1}}{e_{ESTk-1}+e_{MEAk}}\\
&第二步：计算\hat{x}_k=\hat{x}_{k-1}+K_k(z_k-x_{k-1})\\
&第三步：更新e_{ESTk}=(1-K_k)e_{ESTk-1} 后续将给出这个式子详细推导过程

\end{aligned}
$$

### 2.数学基础：数据融合、协方差矩阵、状态空间方程、观测器问题

#### （1）数据融合

再引入一个情境，现在要获取一个物体质量，我手头有两个称A和B，已知两个称都存在误差。A称出来的质量为z₁，标准差为σ₁，B称出来的质量为z₂，标准差为σ₂，两个称的测量结果都符合高斯分布。（z₁和z₂相互独立）

$$
记录估计真实值为\hat{z},求K_k使的σ_{\hat{z}}^2，也就是方差var(\hat{z})最小\\
\begin{aligned}
σ_{\hat{z}}^2&=var(z_1+K_k(z_1-z_2))\\
&=var(z_1-K_kz_1+K_kz_2)\\
&=var((1-K_k)z_1+K_kz_2)\\
&=var((1-K_k)z_1)+var(K_kz_2)\\
&=(1-K_k)^2var(z_1)+K_k^2var(z_2)\\
&=(1-K_k)^2σ_1^2+K_k^2σ_2^2\\
\end{aligned}\\
之后对化简完的式子进行求导,使\frac{dσ_{\hat{z}}^2}{dK_k}=0\\
得到 -σ_1^2+K_kσ_1^2+K_kσ_2^2=0 \\
K_k=\frac{σ_1^2}{σ_1^2+σ_2^2}\\
带入可求\hat{z}最优解(期望)和σ_{\hat{z}}^2
$$

最后大致图解为


#### （2）协方差矩阵

即方差，协方差在一个矩阵中表示出来，表示变量之间的联动关系。（不详细解释了，说的简单，但还是希望有一点点概率论基础）

以三种数据x，y，z举例，数据共有三组即x₁，x₂，x₃，y₁，y₂，y₃，z₁，z₂，z₃
$$
P=\begin{bmatrix}
σ_x^2 & σ_xσ_y & σ_xσ_z \\
σ_yσ_x & σ_y^2 & σ_yσ_z \\
σ_zσ_x & σ_zσ_y & σ_z^2
\end{bmatrix}
$$
提一下如何计算协方差矩阵
$$
记录a为一个过渡矩阵\\
a=\begin{bmatrix}
x_1 & y_1 & z_3 \\
x_2 & y_2 & z_2 \\
x_3 & y_3 & z_3
\end{bmatrix}-\frac{1}{3}\begin{bmatrix}
1 & 1 & 1 \\
1 & 1 & 1 \\
1 & 1 & 1
\end{bmatrix}
\begin{bmatrix}
x_1 & y_1 & z_3 \\
x_2 & y_2 & z_2 \\
x_3 & y_3 & z_3
\end{bmatrix}\\
P=\frac{1}{3}a^Ta
$$

#### （3）状态空间方程和观测器

$$
记录u_k为我们第k次的输入值\\
计算值：x_k=Ax_{k-1}+Bu_k+w_k\\
测量值：z_k=Hz_{k-1}+v_k
$$

$$
由于我们的模型不完全完美，势必会存在不确定性，故有着w_k，v_k两个不确定量，我们分别称为过程噪音和\\
测量噪音。通过一个不太准的计算值和一个不太准的测量值得出一个相对准确的估计值\hat{x}_k就是卡尔曼滤波器的作用
$$

### （4）卡尔曼增益和误差协方差矩阵详细推导（可以跳过直接看公式）

具体过程比较繁琐，下面是我的学习笔记，建议搭配视频食用




### （5）卡尔曼滤波器完整公式

由预测和校正两个部分组成

1.预测
$$
先验估计：\hat{x}_k^-=A\hat{x}_{k-1}+Bu_{k-1}\\
先验误差协方差：P_k^-=AP_{k-1}A^T+Q
$$
2.校正
$$
卡尔曼增益：K_k=\frac{P_k^-H^T}{HP_k^-H^T+R}\\
后验估计：\hat{x}_k=\hat{x}_k^-+K_k(z_k-H\hat{x}_k^-)\\
更新误差协方差：P_k=(I-K_kH)P_k^-
$$

### （6）扩展卡尔曼滤波器（同样可以直接拖到最后看公式）

以上都是卡尔曼滤波器在线性系统中的应用，这里我们说一下卡尔曼滤波器在非线性系统中的应用
$$
\begin{aligned}
x_k&=f(x_{k-1},u_{k-1},w_{k-1})\\
z_k&=h(x_k,v_k)\\
\end{aligned}\\
$$
其中P(w)~N(0,Q); P(v)~N(0,Q)

正态分布的随机变量通过非线性系统后就不再是线性的了，所以我们需要对系统进行线性化，我们这里使用泰勒展开
$$
f(x)=f(x_0)+\frac{\partial{f}}{\partial{x}}(x-x_0)
$$

由于系统存在误差，我们无法在真实点处进行线性化，所以我们退而求其次，在上一次的后验估计处进行线性化
$$
x_k=f(\hat{x}_{k-1},u_{k-1},w_{k-1})+A(x_k-\hat{x}_{k-1})+w_{w_{k-1}}\\
将误差w_{k-1}假设为0，f(\hat{x}_{k-1},u_{k-1},w_{k-1})用\overline{x}_k代替\\
z_k在\overline{x}线性化\\
z_k=h(\overline{x}_k,v_k)+H(x_k-\overline{x}_k)+v_{v_k}\\
将误差v_{k-1}假设为0，h(\overline{x}_k,v_k)用\overline{z}_k代替\\
\begin{aligned}
x_k&=\overline{x}_k+A(x_{k-1}-\overline{x}_{k-1})+w_{w_{k-1}}\\
z_k&=\overline{z}_k+H(x_{k}-\overline{x}_{k})+v_{v_k}\\
\end{aligned}\\
$$
这里P(w_w)~N(0,wQw^T)，P(v_v)同理

经过上述替换，可以得到如下公式

1.预测
$$
先验估计：\hat{x}_k^-=f(\hat{x}_{k-1},u_{k-1},0)\\
先验误差协方差：P_k^-=AP_{k-1}A^T+wQw^T
$$
2.校正
$$
卡尔曼增益：K_k=\frac{P_k^-H^T}{HP_k^-H^T+vRv^T}\\
后验估计：\hat{x}_k=\hat{x}_k^-+K_k(z_k-h(\hat{x}_k^-,0))\\
更新误差协方差：P_k=(I-K_kH)P_k^-
$$