项目进阶——卡尔曼滤波学习路线

卡尔曼滤波学习路线

1. 原理

卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

2. 目的

这篇文章主要是自己同样在学习 Kalman filter 的过程中，总结出一套学习的路线，能够让你在了解 Kalman filter 能够有一个清晰的路线，当然这边也是转载了各位博主的资料，这里说声谢谢。

3. 推荐

视频与博文二选一即可，必看精品哦！

（转载）卡尔曼滤波原理视频（无字幕）

（转载）卡尔曼滤波原理视频（有字幕）

（转载）卡尔曼滤波原理博文

（转载）卡文满滤波原理国外博文（英文）

（转载）如何调参数(R和Q)

4. 公式

4.1 说明

• 出现^代表的是 估计状态。

4.1.1 公式一

$${\hat{x}}_k=F_k\ast {\hat{x}}_{k-1}+{\vec{u}}_k\ast B_k$$

• ${\hat{x}}_k$：当前最优估计（即均值，其他地方常用 $\mu$）
• $F_k$：系数
• ${\hat{x}}_{k-1}$：前一刻最优估计
• $B_k$：系数
• ${\vec{u}}_k$：已知外部控制量

4.1.2 公式二

$$P_k=F_k\ast P_{k-1}\ast F_k^T+Q_k$$

• $P_k$：当前不确定性（估计误差协方差）
• $F_k$：系数
• $P_{k-1}$：前一刻不确定性
• $F_k^T$：系数
• $Q_k$：外部环境干扰

总公式

$$\begin{gathered} {\hat{x}}_k=F_k\ast {\hat{x}}_{k-1}+{\vec{u}}_k\ast B_k \\ {P}_k=F_k\ast P_{k-1}\ast F_k^T+Q_k \end{gathered}$$

由上式可知：

• 当前的最优估计（${\hat{x}}_k$）是根据前一刻最优估计（${\hat{x}}_{k-1}$）预测得到的，并且加上已知外部控制量（${\vec{u}}_k$）的修正。

• 而当前不确定性（$P_k$）由前一刻不确定性（$P_{k-1}$）预测得到的，并加上外部环境干扰（$Q_k$）。

至此，我们对系统可能的动向有了一个模糊的估计，用${\hat{x}}_k$和$P_k$ 来表示。

4.1.3 公式三

如果再结合传感器的数据会怎样呢？
$${\hat{x}}_k^{\prime }={\hat{x}}_k+K^{\prime }\ast ({\vec{z}}_k-H_k\ast {\hat{x}}_k)$$

预测值：粉色

传感器测量值：绿色

• ${\hat{x}}_k^{\prime }$：最新最优估计
• ${\hat{x}}_k$：当前最优估计（即均值，其他地方常用 $\mu$）
• $K^{\prime }$：卡尔曼控制增益
• ${\vec{z}}_k$：分布的均值，读取到的传感器数据
• $H_k$：矩阵（表示传感器数据）

4.1.4 公式四

$$P_k^{\prime }=P_k-K^{\prime }\ast H_k\ast P_k$$

• $P_k^{\prime }$：最新不确定性

• $P_k$：当前不确定性

• $K^{\prime }$：卡尔曼增益

• $H_k$：矩阵（表示传感器数据）

4.1.5 公式五

$$K^{\prime }=P_k\ast {H_k}^T\ast (H_k\ast {H_k}^T\ast P_k+R_k{)}^{-1}$$

• $K^{\prime }$：卡尔曼增益
• $P_k$：当前不确定性
• ${H_k}^T$：
• $H_k$：
• $R_k$：不确定性用协方差（表示传感器噪声）

总公式

$$\begin{gathered} {\hat{x}}_k^{\prime }={\hat{x}}_k+K^{\prime }\ast ({\vec{z}}_k-H_k\ast {\hat{x}}_k) \\ {P}_k^{\prime }=P_k-K^{\prime }\ast H_k\ast P_k \\ {K}^{\prime }=P_k\ast {H_k}^T\ast (H_k\ast {H_k}^T\ast P_k+R_k{)}^{-1} \end{gathered}$$

上式给出了完整的更新步骤方程。就是新的最优估计，我们可以将它和放到下一个预测和更新方程中不断迭代。

调整$K^{\prime }$使得状态误差协方差（$P_k^{\prime }$）最小。（也就是使测量值和估算的误差最小化）

5. 框图

6. 代码

请跳转至，源码MD说明。

/*

举例而言，R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值；反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测。

*/

typedef struct {
    
    
    float x;  /* state */
    float A;  /* x(n)=A*x(n-1)+u(n),u(n)~N(0,q) */
    float H;  /* z(n)=H*x(n)+w(n),w(n)~N(0,r)   */
    float q;  /* process(predict) noise convariance */
    float r;  /* measure noise convariance */
    float p;  /* estimated error convariance */
    float gain;
} kalman1_state;

void kalman1_init(kalman1_state *state, float init_x, float init_p)
{
    
    
    state->x = init_x;  // 待测量的初始值，如有中值一般设成中值（如陀螺仪）
    state->p = init_p;  // 后验状态估计值误差的方差的初始值
    state->A = 1;
    state->H = 1;
    state->q = 2e2;     // 预测噪声协方差
    state->r = 5e2;     // 检测误差协方差
}

float kalman1_filter(kalman1_state *state, float z_measure)
{
    
    
    /* Predict */
    // 公式请对照总公式，就能够清楚知道意思了
    state->x = state->A * state->x;
    state->p = state->A * state->A * state->p + state->q;

    /* Measurement */
    // 公式请对照总公式，就能够清楚知道意思了
    state->gain = state->p * state->H / (state->p * state->H * state->H + state->r);
    state->x = state->x + state->gain * (z_measure - state->H * state->x);
    state->p = (1 - state->gain * state->H) * state->p;

    return state->x;
}

7. MATLAB

MATLAB 是美国 MathWorks 公司出品的商业数学软件，用于数据分析、无线通信、深度学习、图像处理与计算机视觉、信号处理、量化金融与风险管理、机器人，控制系统等领域。在这里，我们用matlab来生成数据波形图，能够更加直观的看到卡尔曼滤波之后的波形。

7.1 使用说明

MATLAB官方为新用户提供了30天免费使用，且无需安装软件，新用户只需要注册并激活，就可以使用了。（这里具体教程就不讲解）matlab官网

那我们这里主要教大家如何使用，添加文件、编译程序、最后生成上面所描述的波形。（同样，也可以使用特定的波形助手工具等方式生成）

• 新建脚本，把代码当中plot_result.m文件内容复制到新建脚本里面，且按Ctrl+s保存脚本，并命名为xxx.m

• 然后重复上面操作新建脚本，且把收集的数据复制到新建脚本里面，且按Ctrl+s保存文件，并命名为result.txt。

• 最后，双击回击到.m文件当中，这个时候运行按键显示正常，点击运行，稍等几秒，将会生成波形图，也就是我们刚开始看到的画面。（如果有同学无法生成最终波形图，可能是由于网络波动导致的）