笔记整理，视频链接 https://www.bilibili.com/video/BV1Rh41117MT?spm_id_from=333.788.videopod.episodes&vd_source=3a22043a6221908dd30039299c0c4976&p=4

1 基础概念

1. 以超声波传感器为例子，横坐标为时间，纵坐标为距离

2. 适用系统：线性高斯系统

• 线性：两个特性
  • 叠加性 ： y = a*x1 + b*x2
  • 齐次性: ky<-----kx
• 高斯：噪声满足正态分布

3. 宏观意义：滤波即是加权

   理想状态： 信号x1 + 噪声*0

   ​ 低 * 1 高 * 0 称之为低通滤波

   ​ 估计值 *（） 观测值*（） 称之为卡尔曼滤波

   ​ |__________________________________________________________________|

​ 修正

2 方程

状态空间表达式：

状态方程：
$$x_k=A{x}_{k-1}+B_{uk}+W_k$$

x_k 当前状态的状态值

x_k-1 上一个时刻的状态值

uk 输入，给到x_k的输入

W_k 过程噪声

A 当前状态上一时刻的值 状态协方差

B 控制矩阵

例子 ： 车从p1 到 p2 , 给到输入 vel

p2 = 1*p1 + dt * vel + wk

wk 相当于 摩擦，顺风，逆风，这些东西

观测方程：
$$y_k=C{x}_k+V_k$$

y_k 观测量

Vk 观测噪声

例子：

火炉对水加温，温度传感器，xk : 水温的状态，yk : 水温的值

观测 ： yk = 1 * xk + vk

状态 : xk = 1*xk-1 + dt + wk

dt : 温度是每时刻变化的

高斯分布

1.直观图解

• 一维 均值 方差
• 二维 投影到x轴 y轴 都是正态分布
• 三维 每个轴都符合高斯分布

参数分析

$$\begin{gathered} W_k\in N(0,Q_k) \\ {V}_k\in N(0,R_k) \end{gathered}$$

Wk ,Vk符合正态分布，均值为0 方差为Qk

通称为高斯白噪声

例子：

Vk Rk 定义

GPS ----> Position
$$1000m+-\delta m{V}_k=\delta m$$
方差为1m噪声
$$R_k=1m$$
Wk Rk

车在行驶，顺风的力
$$5m/s+-\delta m/s{W}_k=\delta m/s$$
方差为1m/s
$$Q_k=1m/s$$
2.方差

• 一维的方差 ： 状态是一个值

  • Qk Rk

  • 状态的方差 估计值的方差，估计值就是状态值，车跑了10m,由于噪声的影响，不是10m 有一个噪声， 10m+wk 本身符合正态分布，所以估计值就有一个方差

• 二维协方差

$$\begin{gathered} x_{t-}=\left[\begin{array}{c}{x}_{t1}\\ x_{t2}\end{array}\right] \\ {W}_{k1} \\ {W}_{k2} \\ Cov(x_{t1},x_{t2})=\left[\begin{array}{cc}cov(x1,x1)& cov(x1,x2)\\ cov(x2,x1)& cov(x2,x2)\end{array}\right] \end{gathered}$$

多维的状态方差也一样，就是维数增多了

3.超参数

Q,R ~ PID

Q : 过程噪声的方差

R : 观测噪声的方差

4**.卡尔曼直观图解**

没有- 表示的是最优估计值，也叫做修正值，有-先验估计值

x_k-1 上一时刻的最优估计值

x_k 基于上一时刻估计出来的当前时刻的值

yk 当前时刻的观测值

当前的估计值，是由 先验估计 和当前的观测值，取公共的部分，得到的

3 公式

1**.卡尔曼公式理解**

实现过程：

使用上一次的最优结果预测当前的值（先验估计）

同时使用观测值（传感器传回来的那个值）修正当前值（先验估计），得到最优结果

预测：
$$\begin{gathered} {\hat{x}}_k^{-}=F{\hat{x}}_{k-1}+B{u}_{k-1} \\ {\hat{P}}_k^{-}=F\ast P_{k-1}\ast F^T+Q \end{gathered}$$
更新：
$$\begin{gathered} K_k={\hat{P}}^{-}\ast H_k^T\ast (H_k\ast {\hat{P}}^{-}\ast H_k^T+{\hat{R}}^{-}{)}^{-1} \\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{\hat{x}}_k^{-}) \\ {P}_k=(I-K_k\ast H)P_k^{-} \end{gathered}$$
例子：

车 ，有两个状态 p位置 v速度
$$x_t=\left[\begin{array}{c}P\\ V\end{array}\right]$$

运动模型 ： 匀加速直线运动

预测模型：

$$\begin{gathered} P_i=P_{i-1}+V_{i-1}\ast \delta t+\frac{a}{2}\ast \delta t^2 \\ {V}_i=V_{i-1}+a\ast \delta t \\ \left[\begin{array}{c}{P}_i\\ V_i\end{array}\right]=\left[\begin{array}{cc}1& \delta t\\ 0& 1\end{array}\right]\ast \left[\begin{array}{c}{P}_{i-1}\\ V_{i-1}\end{array}\right]+\left[\begin{array}{c}\frac{1}{2}\ast \delta t^2\\ \delta t\end{array}\right]\ast a_i \\ ={\hat{x}}_t^{-}=F\ast {\hat{x}}_{t-1}+B\ast u_{t-1} \end{gathered}$$

先验估计：

$${\hat{x}}_t^{-}=F\ast {\hat{x}}_{t-1}+B\ast u_{t-1}+W_k$$
先验估计协方差：
$${\hat{P}}_t^{-}=F\ast P_{t-1}\ast F^T+Q$$

F 状态转移矩阵

基础知识：
$$\begin{gathered} cov(x,x)=Var(x) \\ cov(Ax,Ax)=A\ast cov(x,x)\ast A^T \\ cov(Ax+k,Ax+k)=A\ast cov(x,x)\ast A^T \end{gathered}$$
所以
$$\begin{gathered} cov({\hat{x}}_t^{-},{\hat{x}}_t^{-})=cov(F\ast {\hat{x}}_{t-1}+B\ast u_{t-1}+W_k,F\ast {\hat{x}}_{t-1}+B\ast u_{t-1}+W_k) \\ =F\ast cov({\hat{x}}_{t-1},{\hat{x}}_{t-1})\ast F^T+cov(W_k,W_k) \\ =F\ast P_{t-1}\ast F^T+Q \end{gathered}$$

测量模型：
$$\begin{gathered} Z_p=P_t+\Delta P_t \\ {Z}_v=0 \\ \left[\begin{array}{c}{Z}_p\\ Z_v\end{array}\right]=\left[\begin{array}{cc}1& 0\end{array}\right]\ast \left[\begin{array}{c}{P}_t\\ V_t\end{array}\right]+\left[\begin{array}{cc}1& 0\end{array}\right]\ast \left[\begin{array}{c}\Delta P_t\\ \Delta V_t\end{array}\right] \\ =Z_t=[1,0]x_t+\Delta P_t \\ =H\ast x_t+v \end{gathered}$$
测量方程：
$$Z_t=H\ast x_t+v$$

Z_t 的维数不一定跟 x_t 相同，比如z_v 为0 ，所以可以只写z_p

修正估计：
$${\hat{x}}_t={\hat{x}}_t^{-}+K_k(z_t-H{\hat{x}}_t^{-})$$

最终滤波结果

$z_t-H{\hat{x}}_t^{-}$ 测量和预测的差值

更新卡尔曼增益：
$$\begin{gathered} K_k={\hat{P}}^{-}\ast H_k^T\ast (H_k\ast {\hat{P}}^{-}\ast H_k^T+{\hat{R}}^{-}{)}^{-1} \\ ={\widehat{P_t}}^{-}\ast ({\hat{P}}_t^{-}+{\hat{R}}^{-}{)}^{-1} \\ =(P_{t-1}+Q)\ast (P_{t-1}+Q+R{)}^{-1} \end{gathered}$$
更新后验估计协方差

后验是融合了传感器观测值的
$$P_t=(I-K_t\ast H)P_t^{-}$$

2. 调节超参数

   • Q 与 R 的取值

     F，H 取 1，化简得到
     $$K=(P_{t-1}+Q)\ast (P_{t-1}+Q+R{)}^{-1}$$
     结合：
     $${\hat{x}}_t={\hat{x}}_t^{-}+K_k(z_t-H{\hat{x}}_t^{-})$$
     当需要更信任观测值Z， 那么K值要增大，比如说你的传感器精度高，那么就可以让R小一点

     比如说运动模型很理想，Q,R 都可以调节的大一些

   • 分析Q

     比如车走在路上，基本没有摩擦，那么Q就可以取的小一点，如果模型不准确，那么就要调节的大一点

   • 分析R

     观测噪声的方差，比如说陀螺仪传感器很贵的时候，R就可以小一点，因为方差小，如果很便宜，R就要大一点

P0 与 x0 的取值

习惯取 x0 = 0, P往小的取，方便收敛，（一般取1，不可为0）

3. 卡尔曼滤波的使用
   • 选择状态量，观测量
   • 构建方程
   • 初始化参数
   • 代入公式迭代
   • 调节超参数

4 应用

离散系统

系统描述
$$x(k)=Ax(k-1)+Bu(k)(+W(k))$$
测量值
$$Z(k)=Hx(k)+V(k)$$
系统预测

1.先验估计
$$x(k|k-1)=Ax(k-1|k-1)+Bu(k)$$

x(k-1) 预测结果

Ax(k-1|k-1) ： k-1态最优结果

Bu(k) ： 状态控制量

2.误差协方差
$$P(k|k-1)=AP(k-1|k-1)A^T+Q$$

x(k | k-1) ------ cov[x(k | k-1)] —>P(k |k-1)

Ax(k-1|k-1) ----- cov[ Ax(k-1|k-1) ] — > AP(k-1 | k-1)A^T

测量方程

3.建立测量方程
$$Z(k)=Hx(k)+V(k)$$
结合观测值计算最优估计值x(k | k)

生成最优估计

4.计算卡尔曼增益
$$Kg(k)=(P(k|k-1)\ast H^T)\ast (HP(k|k-1)\ast H^T+R{)}^{-1}$$

5.修正估计
$$x(k|k)=x(k|k-1)+Kg(k)\ast [Z(k)-Hx(k|k-1)]$$

6.更新误差协方差
$$P(k|k)=[1-Kg(k)\ast H]\ast P(k|k-1)$$
机器人应用案例

1 陀螺仪滤波

明确以下参数

陀螺仪噪声协方差 Q_angle

陀螺仪漂移噪声协方差 Q_gyro

加速度计协方差 R_angle

陀螺仪测得角速度 newGyro

采样周期 dt

状态量：
$$\left[\begin{array}{c}angle\\ Q\_bias\end{array}\right]--->\left[\begin{array}{c}{Q}_{a}ngle\\ Q\_gyro\end{array}\right]$$

观测量：
newGyro ---->R_angle

1. 预测当前角度值

由先验估计方程式：
$$x(k|k-1)=Ax(k-1|k-1)+Bu(k)$$

$$\left[\begin{array}{c}{P}_i\\ V_i\end{array}\right]=\left[\begin{array}{cc}1& \delta t\\ 0& 1\end{array}\right]\ast \left[\begin{array}{c}{P}_{i-1}\\ V_{i-1}\end{array}\right]+\left[\begin{array}{c}\frac{1}{2}\ast \delta t^2\\ \delta t\end{array}\right]\ast a_i$$
预测当前角度值：
$$\left[\begin{array}{c}angle\\ Q\_bias\end{array}\right]=\left[\begin{array}{cc}1& -dt\\ 0& 1\end{array}\right]\ast \left[\begin{array}{c}angle\\ Q\_bias\end{array}\right]+\left[\begin{array}{c}dt\\ 0\end{array}\right]\ast newGyro$$

运动方程：
$$\begin{gathered} angl{e}_i=angl{e}_{i-1}-Q\_bias\ast dt+newGyro\ast dt \\ Q\_bia{s}_i=Q\_bia{s}_{i-1} \end{gathered}$$
2.预测协方差矩阵

由误差协方差
$$P(k|k-1)=AP(k-1|k-1)A^T+Q$$
由 先验估计有系统参数：
$$A=\left[\begin{array}{cc}1& -dt\\ 0& 1\end{array}\right]$$
系统过程协方差噪声Q定义
$$\left[\begin{array}{cc}cov(angle,angle)& cov(Q\_bias,angle)\\ cov(Q\_bias,angle)& cov(Q\_bias,Q\_bias)\end{array}\right]$$
角度噪声跟角速度漂移噪声独立
$$\left[\begin{array}{cc}D(angle)& 0)\\ 0& D(Q\_bias)\end{array}\right]=\left[\begin{array}{cc}Q\_angle& 0)\\ 0& Q\_gyro\end{array}\right]$$
Q_angle, Q_gyro 的方差为常数，可由于经验值或者计算得出

设上一次预测协方差矩阵Pk-1为
$$P_{k-1}=\left[\begin{array}{cc}{a}_{k-1}& b_{k-1}\\ c_{k-1}& d_{k-1}\end{array}\right]$$
本次预测协方差矩阵Pk为：
$$P_k=\left[\begin{array}{cc}{a}_k& b_k\\ c_k& d_k\end{array}\right]$$
将以上参数带入预测协方差公式:
$$\begin{gathered} \left[\begin{array}{cc}{a}_k& b_k\\ c_k& d_k\end{array}\right]=\left[\begin{array}{cc}1& -dt\\ 0& 1\end{array}\right]\ast \left[\begin{array}{cc}{a}_{k-1}& b_{k-1}\\ c_{k-1}& d_{k-1}\end{array}\right]\ast \left[\begin{array}{cc}1& 0\\ -dt& 1\end{array}\right]+\left[\begin{array}{cc}D(angle)& 0)\\ 0& D(Q\_bias)\end{array}\right] \\ =\left[\begin{array}{cc}{a}_{k-1}\ast c_{k-1}\ast dt-b_{k-1}\ast dt+d_{k-1}\ast d{t}^2& b_{k-1}-d_{k-1}\ast dt\\ c_{k-1}-d_{k-1}\ast dt& d_{k-1}\end{array}\right]+\left[\begin{array}{cc}D(angle)& 0)\\ 0& D(Q\_bias)\end{array}\right] \end{gathered}$$

3. 建立测量方程

   系统测量方程:
   $$Z(k)=Hx(k)+V(k)$$
   系统测量系数
   $$H=[1,0]$$
   code:
   $$measure=newAngle$$

4 计算卡尔曼增益

由于卡尔曼增益
$$Kg(k)=(P(k|k-1)\ast H^T)\ast (HP(k|k-1)\ast H^T+R{)}^{-1}$$

由于卡尔曼系数包括角度和陀螺仪漂移：
$$\left[\begin{array}{c}k0\\ k1\end{array}\right]=\frac{\left[\begin{array}{cc}a& b\\ c& d\end{array}\right]\ast \left[\begin{array}{c}1\\ 0\end{array}\right]}{[1,0]\ast \left[\begin{array}{cc}a& b\\ c& d\end{array}\right]\ast \left[\begin{array}{c}1\\ 0\end{array}\right]+R\_angle}$$
角度测量噪声R_angle 为常数

5. 计算当前最优化估计值
   $$x(k|k)=x(k|k-1)+Kg(k)\ast [Z(k)-Hx(k|k-1)]$$

推出：
$$\left[\begin{array}{c}angle\\ Q\_bias\end{array}\right]=\left[\begin{array}{c}angle\\ Q\_bias\end{array}\right]+\left[\begin{array}{c}k0\\ k1\end{array}\right]\ast (newGyro-\left[\begin{array}{c}1\\ 0\end{array}\right]\ast \left[\begin{array}{c}angle\\ Q\_bias\end{array}\right])$$

6. 更新协方差矩阵
   $$P(k|k)=[1-Kg(k)\ast H]\ast P(k|k-1)$$

推出：
$$\left[\begin{array}{cc}{a}_k& b_k\\ c_k& d_k\end{array}\right]=[\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]-\left[\begin{array}{c}k0\\ k1\end{array}\right]\ast [1,0]]\ast \left[\begin{array}{cc}{a}_k& b_k\\ c_k& d_k\end{array}\right]$$