卡尔曼滤波器原理By_DR_CAN 学习笔记

DR_CAN卡尔曼滤波器

Kalman Filter
Optimal Recursive Data Processing Algorithm

不确定性：
1、不存在完美的数学模型
2、系统的扰动不可控，也很难建模
3、测量传感器存在误差

Recursive Algorithm

第 K 次测量结果 $z_k$
通过样本均值来估计真实值
估计值 ${\hat{x}}_k=\frac{1}{k}(z_1+z_2+...+z_k)=\frac{1}{k}(z_1+z_2+...+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}(z_k?{\hat{x}}_{k?1})$

$k\to \infty ,\frac{1}{k}\to 0,{\hat{x}}_k={\hat{x}}_{k?1}$ 测量结果不重要
$k\to 0,\frac{1}{k}\to \infty$ 测量值$z_k$作用较大

令 $k_k=\frac{1}{k}$，其中$k_k$为Kalman Gain
${\hat{x}}_k={\hat{x}}_{k?1}+k_k(z_k?{\hat{x}}_{k?1})$

当前估计值 = 上次估计值 + 系数 *（当前测量值-上次估计值）
估计误差 $e_{EST}$
测量误差 $e_{MEA}$
Klaman Gain $k_k=\frac{e_{ES{T}_{k?1}}}{e_{ES{T}_{k?1}}+e_{ME{A}_k}}$

① $e_{ES{T}_{k?1}}?e_{ME{A}_k},k_k\to 1,{\hat{x}}_k=z_k$ 估计误差大，估计值取测量值
② $e_{ES{T}_{k?1}}?e_{ME{A}_k},k_k\to 0,{\hat{x}}_k={\hat{x}}_{k?1}$ 测量误差大，估计值取上次估计值

迭代过程

STEP1：计算 Kalman Gain $k_k=\frac{e_{ES{T}_{k?1}}}{e_{ES{T}_{k?1}}+e_{ME{A}_k}}$
STEP2：计算${\hat{x}}_k={\hat{x}}_{k?1}+k_k(z_k?{\hat{x}}_{k?1})$
STEP3：更新$e_{ES{T}_k}=(1?k_k)e_{ES{T}_{k?1}}$

数学基础

正态分布和6-Sigma

通过测量获取多组数据，用样本均值代替期望，样本方差代替方差

样本方差 $S^2=\frac{1}{n}{\sum }_{i=1}^n(x_i?\hat{x}{)}^2$
样本标准差 $S=\sqrt{\frac{1}{n}{\sum }_{i=1}^n(x_i?\hat{x}{)}^2}$

理论正态分布

实际修正后的对应表格

$\sigma$	产出百分比	瑕疵百分比
3	93.3 %	6.7%
5	99.977%	0.023 %
6	99.99966%	0.00034%

生产设备

每日生产量	3$\sigma$	5$\sigma$	6$\sigma$
200	1天13次	21天1次	4年1次

false dismissal 漏检
将某些不合格视为合格

false alarm 假警报
将某些合格视为不合格

Data Fusion

Measurement：$z_1=30g$ Standard Deviation：${\sigma }_1=2g$
Measurement：$z_2=32g$ Standard Deviation：${\sigma }_1=4g$
两者均遵从 Natural Distribution

估计真实值 $\hat{z}$
$\hat{z}=z_1+k(z_2?z_1)，k\in [0,1]$
求 k 使得${\sigma }_{\hat{z}}$最小
${\sigma }_{\hat{z}}^2=Var(z_1+k(z_2?z_1))=Var[(1?k)z_1+k{z}_2]=Var[(1?k)z_1]+Var(k{z}_2)=(1?k{)}^2{\sigma }_1^2+k^2{\sigma }_2^2$

$\frac{d{\sigma }_{\hat{z}}^2}{dk}=?2(1?k){\sigma }_1^2+2k{\sigma }_2^2=0$
$k=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}$

将数据带入
$k=\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}=\frac{4}{4+16}=0.2$
$\hat{z}=30+0.2?(32?30)=30.4$ 最优解
${\sigma }_{\hat{z}}^2=0.8^2?4+0.2^2?16=3.2$

Covariance Matrix

将方差、协方差在一个矩阵中表现出来，体现变量间的联动关系

球员	身高(x)	体重(y)	年龄(z)
瓦尔迪	179	74	33
奥巴梅扬	187	80	31
萨拉赫	175	71	28
平均	180.3	75	30.7

方差 ${\sigma }_x^2=\frac{1}{3}[(179?180.3{)}^2+(187?180.3{)}^2+(175?180.3{)}^2]=24.89$
${\sigma }_y^2=14$
${\sigma }_z^2=4.22$

协方差 ${\sigma }_x{\sigma }_y=\frac{1}{3}[(179?180.3)(74?75)+(187?180.3)(80?75)+(175?180.3)(71?75)]=18.7={\sigma }_y{\sigma }_x$
${\sigma }_x{\sigma }_z=4.4={\sigma }_z{\sigma }_x$
${\sigma }_y{\sigma }_z=3.3={\sigma }_z{\sigma }_y$

Covariance matrix $P=\left[\begin{array}{ccc}{\sigma }_x^2& {\sigma }_x{\sigma }_y& {\sigma }_x{\sigma }_z\\ {\sigma }_y{\sigma }_x& {\sigma }_y^2& {\sigma }_y{\sigma }_z\\ {\sigma }_z{\sigma }_x& {\sigma }_z{\sigma }_y& {\sigma }_z^2\end{array}\right]$

$$过度矩阵a=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]?\frac{1}{3}\left[\begin{array}{ccc}1& 1& 1\\ 1& 1& 1\\ 1& 1& 1\end{array}\right]\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$$

$P=\frac{1}{3}{a}^{T}a$

${\sigma }_x{\sigma }_y$表示协方差，不是两个标准差相乘

State Space Representation

$m\ddot{x}=F?kx?b\dot{x}$
F 为 Input，记为u
$m\ddot{x}=u?kx?b\dot{x}$

取 State Variable
$x_1=x$
$x_2=\dot{x}$

${\dot{x}}_1=\dot{x}=x_2$
${\dot{x}}_2=\ddot{x}=\frac{1}{m}u?\frac{k}{m}x?\frac{b}{m}\dot{x}=\frac{1}{m}u?\frac{k}{m}{x}_1?\frac{b}{m}{x}_2$

$$\left(\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\end{array}\right)=\left(\begin{array}{cc}0& 1\\ ?\frac{k}{m}& ?\frac{b}{m}\end{array}\right)\left(\begin{array}{c}{x}_1\\ x_2\end{array}\right)+\left(\begin{array}{c}0\\ \frac{1}{m}\end{array}\right)u$$

记作 随时间变化 $\dot{X}(t)=AX(t)+BU(t)$

测量量Measurement
$z_1=x=x_1位置$
$z_2=\dot{x}=x_2速度$

$$\left(\begin{array}{c}{z}_1\\ z_2\end{array}\right)=\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right)\left(\begin{array}{c}{x}_1\\ x_2\end{array}\right)$$

记作 Z(t) = HX(t)

整个过程充满不确定性，故状态空间方程离散化表达式为
计算值/估计值 $X_k=A{X}_{k?1}+B{U}_k+W_{k?1}$
测量值 $Z_k=H{X}_k+V_k$

$W_{k?1}$ 记作过程噪音 Process Noise
$V_k$ 记作测量噪音 Measurement Noise

可通过前面介绍的数据融合，将估计值与测量值融合，得到更加可信赖的目标估计值

离散化推导

通过欧拉法（前向差分）将状态空间方程离散化
$\dot{X}=\frac{X_{k+1}?X_k}{T}=A{X}_k+B{U}_k$
$X_{k+1}=(TA+I)X_k+TB{U}_k$
记作 $X_{k+1}=\Phi X_k+G{U}_k$

故 $X_k=\Phi X_{k?1}+G{U}_{k?1}+W_{k?1}$

linearization

Taylor Series

线性系统 linear system $?$ 叠加原理 superposition

$\dot{x}=f(x)$
① $x_1,x_2$是解
② $x_3=k_1{x}_1+k_2{x}_2(k_1,k_2为constant)$
③ $x_3$是解

线性化：Taylor Series
是在某一点附近的线性化，而不是全局线性化

$f(x)=f(x_0)+\frac{f^{\prime }(x_0)}{1!}(x?x_0)+\frac{f^{\prime \prime }(x_0)}{2!}(x?x_0{)}^2+...+\frac{f^{(n)}(x_0)}{n!}(x?x_0{)}^n$
若 $x?x_0\to 0$，则$(x?x_0{)}^2\to 0$，$(x?x_0{)}^n\to 0$
故 $f(x)=f(x_0)+f^{\prime }(x_0)(x?x_0)$ 即 $f(x)=k_{2}x+b$

平衡点
$\ddot{x}+\dot{x}+\frac{1}{x}=1$ 在平衡点（Fixed Point）附近线性化
令 $\ddot{x}=\dot{x}=0$ $则平衡点x_0=1$
around $x_0：x_{\delta }=x_0+x_d$
${\ddot{x}}_{\delta }+{\dot{x}}_{\delta }+\frac{1}{x_{\delta }}=1$
$\frac{1}{x_{\delta }}的线性化\frac{1}{x_{\delta }}=\frac{1}{x_0}?\frac{1}{x_0^2}(x_{\delta }?x_0)=1?x_d$

故${\ddot{x}}_d+{\dot{x}}_d+1?x_d=1$
即${\ddot{x}}_d+{\dot{x}}_d?x_d=0$

2-D

${\dot{x}}_1=f_1(x_1,x_2)$
${\dot{x}}_2=f_2(x_1,x_2)$

$\left[\begin{array}{c}{\dot{x}}_{1d}\\ {\dot{x}}_{2d}\end{array}\right]={\left[\begin{array}{cc}\frac{?f_1}{?x_1}& \frac{?f_1}{?x_2}\\ \frac{?f_2}{?x_1}& \frac{?f_2}{?x_2}\end{array}\right]}_{|x=x_0}\left[\begin{array}{c}{x}_{1d}\\ x_{2d}\end{array}\right]$

$\ddot{x}+\dot{x}+\frac{1}{x}=1$
Let $x_1=x，x_2=\dot{x}$
$$\begin{gathered} {\dot{x}}_1=\dot{x}=x_2 \\ {\dot{x}}_2=\ddot{x}=?\dot{x}?\frac{1}{x}+1 \end{gathered}$$

令 ${\dot{x}}_1={\dot{x}}_2=0$，平衡点$x_{10}=1,x_{20}=0$
$\left[\begin{array}{c}{\dot{x}}_{1d}\\ {\dot{x}}_{2d}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 1& ?1\end{array}\right]\left[\begin{array}{c}{x}_{1d}\\ x_{2d}\end{array}\right]$

即
$$\begin{gathered} {\dot{x}}_{1d}=x_{2d} \\ {\dot{x}}_{2d}=x_{1d}?x_{2d} \\ {\ddot{x}}_d=x_d?{\dot{x}}_d?{\ddot{x}}_d+{\dot{x}}_d?x_d=0 \end{gathered}$$

Summary

1-D
$f(x)=f(x_0)+f^{\prime }(x_0)(x?x_0),x?x_0\to 0$

2-D
$\left[\begin{array}{c}{\dot{x}}_{1d}\\ {\dot{x}}_{2d}\end{array}\right]={\left[\begin{array}{cc}\frac{?f_1}{?x_1}& \frac{?f_1}{?x_2}\\ \frac{?f_2}{?x_1}& \frac{?f_2}{?x_2}\end{array}\right]}_{|x=x_0\to FixedPoint}\left[\begin{array}{c}{x}_{1d}\\ x_{2d}\end{array}\right]$

Step by Step Derivation of Kalman Gain

$$\begin{gathered} X_k=A{X}_{k?1}+B{U}_{k?1}+W_{k?1} \\ {Z}_k=H{X}_k+V_k \end{gathered}$$

A为状态矩阵 B为控制矩阵

P(W) ~ N(0, Q) ，W为过程噪声
Q 为协方差矩阵，$Q=E[W{W}^T]$

P(v) ~ N(0, R)，V为测量噪声
R 为协方差矩阵，$R=E[V{V}^T]$

先验估计值 ${\hat{X}}_k^{?}=A{X}_{k?1}+B{U}_{k?1}$
测量值预测值 $Z_k=H{X}_k?{\hat{X}}_{kMEA}=H^{?1}{Z}_k$

${\hat{X}}_k={\hat{X}}_k^{?}+G(H^{?1}{Z}_k?{\hat{X}}_k^{?}),G\in [0,1]$
令 $G=K_{k}H$，带入上式可得：
${\hat{X}}_k={\hat{X}}_k^{?}+K_k(Z_k?H{\hat{X}}_k^{?}),K_k\in [0,H^{?1}]$

目标： 寻找$K_k，使得{\hat{X}}_k\to X_k，即使得Var({\hat{X}}_k)最小$

$e_k=X_k?{\hat{X}}_k$
$P(e_k)$ ~ N(0, P)
P 为误差协方差矩阵，$P=E(e{e}^T)$
$tr(P)最小时，方差最小$

$P=E(e{e}^T)=E[(X_k?{\hat{X}}_k)(X_k?{\hat{X}}_k{)}^T]$

$$\begin{gathered} X_k?{\hat{X}}_k=X_k?[{\hat{X}}_k^{?}+K_k(Z_k?H{\hat{X}}_k^{?})] \\ =X_k?{\hat{X}}_k^{?}?K_k{Z}_k+K_{k}H{\hat{X}}_k^{?} \\ =X_k?{\hat{X}}_k^{?}?K_k(H{X}_k+V_k)+K_{k}H{\hat{X}}_k^{?} \\ =(X_k?{\hat{X}}_k^{?})?K_{k}H(X_k?{\hat{X}}_k^{?})?K_k{V}_k \\ =（I?K_{k}H)(X_k?{\hat{X}}_k^{?})?K_k{V}_k \\ =(I?K^{k}H)e_k^{?}?K_k{V}_k \end{gathered}$$
其中 $e_k^{?}称为先验误差$

$$\begin{gathered} E[(I?K_{k}H)e_k^{?}?K_k{V}_k][(I?K_{k}H)e_k^{?}?K_k{V}_k{]}^T \\ =E[(I?K_{k}H)e_k^{?}?K_k{V}_k][e_k^{?T}(I?K_{k}H{)}^T?V_k^T{K}_k^T] \\ =E[(I?K_{k}H)e_k^{?}{e}_k^{?T}(I?K_{k}H{)}^T?(I?K_{k}H)e_k^{?}{V}_k^T{K}_k^T?K_k{V}_k{e}_k^{?T}(I?K_{k}H{)}^T+K_k{V}_k{V}_k^T{K}_k^T] \end{gathered}$$

$$\begin{gathered} E(I?K_{k}H)e_k^{?}{V}_k^T{K}_k^T \\ =(I?K_{k}H)E(e_k^{?}{V}_k^T)K_k^T \\ =(I?K_{k}H)E(e_k^{?})E(V_k^T)K_k^T=0 \end{gathered}$$

故$$\begin{gathered} P_k=(I?K_{k}H)E(e_k^{?}{e}_k^{?T})(I?K_{k}H{)}^T+K_{k}E(V_k{V}_k^T)K_k^T \\ =(I?K_{k}H)P_k^{?}(I?K_{k}H{)}^T+K_{k}R{K}_k^T \\ =(P_k^{?}?K_{k}H{P}_k^{?})(I?H^T{K}_k^T)+K_{k}R{K}_k^T \\ =P_k^{?}?P_k^{?}{H}^T{K}_k^T?K_{k}H{P}_k^{?}+K_{k}H{P}_k^{?}{H}^T{K}_k^T+K_{k}R{K}_k^T \end{gathered}$$

$tr(P_k)=tr(P_k^{?})?2tr(K_{k}H{P}_k^{?})+tr(K_{k}H{P}_k^{?}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)$

矩阵求导公式

$\frac{dtr(AB)}{dA}=B^T$
$\frac{dtr(AB{A}^T)}{dA}=2AB$

令 $$\begin{gathered} \frac{dtr(P_k)}{d{K}_k}=0?2(H{P}_k^{?}{)}^T+2K_{k}H{P}_k^{?}{H}^T+2K_{k}R \\ =?P_k^{?}{H}^T+K_k(H{P}_k^{?}{H}^T+R)=0 \end{gathered}$$

则 $K_k=\frac{P_k^{?}{H}^T}{H{P}_k^{?}{H}^T+R}$
$R$ 为测量噪声协方差矩阵
$P_k^{?}$ 为先验误差协方差矩阵

Prior / Posterior Error Covariance Matrix

$X_k=A{X}_{k?1}+B{U}_{k?1}+W_{k?1}，W$~$P(0,Q)$
$Z_k=H{W}_k+V_k，V$~$P(0,R)$

先验估计
${\hat{X}}_k^{?}=A{\hat{X}}_{k?1}+B{U}_{k?1}$

后验估计
${\hat{X}}_k={\hat{X}}_k^{?}+K_k(Z_k?H{\hat{X}}_k^{?})$

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

求$P_k^{?}$

误差 $e_k=X_k?{\hat{X}}_k$
先验误差 $$\begin{gathered} e_k^{?}=X_k?{\hat{X}}_k^{?}=A{X}_{k?1}+B{U}_{k?1}+W_{k?1}?A{\hat{X}}_{k?1}?B{U}_{k?1} \\ =A(X_{k?1}?{\hat{X}}_{k?1})+W_{k?1}=A{e}_{k?1}+W_{k?1} \end{gathered}$$

$$\begin{gathered} P_k^{?}=E[e_k^{?}{e}_k^{?T}] \\ =E[(A{e}_{k?1}+W_{k?1})(e_{k?1}^T{A}^T+W_{k?1}^T)] \\ =E[A{e}_{k?1}{e}_{k?1}^T{A}^T+A{e}_{k?1}{W}_{k?1}^T+W_{k?1}{e}_{k?1}^T{A}^T+W_{k?1}{W}_{k?1}^T] \\ =AE(e_{k?1}{e}_{k?1}^T)A^T+E(W_{k?1}{W}_{k?1}^T) \\ =A{P}_{k?1}{A}^T+Q \end{gathered}$$

使用卡尔曼滤波器 估计 状态变量的值（最终五个式子）

• 预测
  – 先验：${\hat{X}}_k^{?}=A{\hat{X}}_{k?1}+B{U}_{k?1}$
  – 先验误差协方差矩阵：$P_k^{?}=A{P}_{k?1}{A}^T+Q$

• 校正
  – 卡尔曼增益：$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^{?})$
  – 更新误差协方差
  $$\begin{gathered} P_k=P_k^{?}?P_k^{?}{H}^T{K}_k^T?K_{k}H{P}_k^{?}+K_{k}H{P}_k^{?}{H}^T{K}_k^T+K_{k}R{K}_k^T \\ =P_k^{?}?P_k^{?}{H}^T{K}_k^T?K_{k}H{P}_k^{?}+K_k(H{P}_k^{?}{H}^T+R)K_k^T \\ =P_k^{?}?P_k^{?}{H}^T{K}_k^T?K_{k}H{P}_k^{?}+P_k^{?}{H}^T{K}_k^T \\ =P_k^{?}?K_{k}H{P}_k^{?} \\ =(I?K_{k}H)P_k^{?} \end{gathered}$$

An 2D example

excel 矩阵处理函数
矩阵相乘 mmul()
转换 transpose()
取逆 minverse()

全选F2
Ctrl+Shift+Enter

该案例需要赋初始值变量
$$\begin{gathered} 状态变量初始值x_{1,0},x_{2,0} \\ 后验估计值初始值{\hat{x}}_{1,0},{\hat{x}}_{2,0} \\ 误差协方差矩阵P_0 \end{gathered}$$

Extended Kalman Filter(EKF)

理解

卡尔曼增益K：负责融合估计值与测量值，谁方差小，就更相信谁一些。

状态估计协方差矩阵P：初始状态与过程噪声有关，并且由于每次K的迭代，状态估计协方差矩阵也在迭代（因为使用了上一次的结果作为下一次的初始状态，上一次的结果来自于卡尔曼增益K对观测与估计的融合，状态估计协方差会减小）

过程噪声协方差矩阵Q：来自于世界中的不确定性，这个值怎么选，我也不太清楚，不知道如何度量世界中的不确定性。

测量误差协方差矩阵R：来自于传感器误差（我猜是可以通过试验获得，比如测量100次，然后记录数据）

其余的就是要给一个目标的初始状态，然后根据观测，不断地更新估计，得到一个稳定的K。