卡尔曼滤波学习笔记

卡尔曼滤波

前言

​ 最近在学习卡尔曼滤波算法，这其中用到了一些数学和现代控制理论的知识。由于我在学习之前没有啥知识储备，在网上看了很多博客、花了很多时间之后依然是一种似懂非懂的感觉。终于找到了一些很棒的教程，感觉算是稍微理解了一些，整理了一些学习笔记，梳理思路，也方便日后查阅。

教程链接

• 【卡尔曼滤波器】1_递归算法_Recursive Processing_哔哩哔哩_bilibili
• 从放弃到精通！卡尔曼滤波从理论到实践~_哔哩哔哩_bilibili
• https://blog.csdn.net/Yemiekai/article/details/116372288

数学基础

协方差矩阵

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

假设有X,Y,Z三组数据，每组数据有n个

方差：

$$\sigma_x^2 = \frac{1}{n}\sum(x_i - \bar{x})^2$$

方差越大，数据波动越大

协方差：

$$\sigma_x\sigma_y = \sigma_y\sigma_x = \frac{1}{n}\sum(x_i - \bar{}x)(y_i - \bar{y})$$

协方差越小，X,Y相关性越低

协方差矩阵：

$$\begin{bmatrix}\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{bmatrix}$$

过渡矩阵

过渡矩阵a用以计算协方差矩阵P：

$$P = \frac{1}{n}a^Ta$$

结合公式容易理解：(此处为3x3矩阵)

$$a = \begin{bmatrix}x1 & y1 & z1 \\ x2 & y2 & z2 \\ x3 & y3 & z3 \\ \end{bmatrix} - \frac{1}{3}\begin{bmatrix}1 & 1 & 1 \\ 1 & 1 & 1 \\ 1 & 1 & 1 \\ \end{bmatrix}\begin{bmatrix}x1 & y1 & z1 \\ x2 & y2 & z2 \\ x3 & y3 & z3 \\ \end{bmatrix}$$

被减数即为原矩阵，而减数则为平均值矩阵

状态空间方程

$$\hat{X}_k = A\hat{X}_{k-1} + Bu_{k-1} + W_{k-1}$$ $$Z_k = HX_k + V_k$$

变量说明：

• $\hat{X}_k$：实际状态值
• $\hat{X}_{k-1}$：上一时刻状态值
• $u_k-1$：控制输入量
• A、H、B：转换矩阵
• $Z_k$：测量值
• $W_{k-1}$：过程噪声
• $V_k$：测量噪声

关于传递方程：

​ 如果我们掌握了一个物体的运动规律，那么这个时刻的状态可以通过上一个时刻的状态计算出来。但是也不能保证完全精准，所以添加了过程噪声——W

​ 举个例子——经典的阻尼弹簧振子模型：

弹簧恢复力 = kX，阻力 = Bv。将X的导数v记作$\dot{X}$，X的二阶导a记作$\ddot{X}$，便可以得到：

$$m\ddot{X} + B\dot{X} + kX = F$$

取状态变量X1为位移，X2为速度，F记为控制输入u，根据之前公式可得：

$$\dot{X_1} = X_2$$ $$\dot{X_2} = \ddot{X} = \frac{1}{m}u - \frac{B}{m}X_2 - \frac{k}{m}X_1$$

写成矩阵的形式就是：

$$\begin{bmatrix} \dot{X_1} \\ \dot{X_2} \end{bmatrix} = \begin{bmatrix} 0 & 1 \\ -\frac{k}{m} & -\frac{B}{m} \end{bmatrix}\begin{bmatrix} X_1 \\ X_2 \end{bmatrix} + \begin{bmatrix} 0 \\ \frac{1}{m} \end{bmatrix}u$$

对应着状态趋势矩阵与状态的关系：

$$\dot{X_t} = AX_t + Bu_t$$

写成离散的形式就是：

$$X_k = AX_{k-1} + Bu_{k}$$

当然最后加上不确定因素W

关于测量方程：

​ 一般而言我们可能会习惯把待求量放在等式左边，已知量放在等式右边。但是在这里$Z_k$才是我们实际获得的数据。

​ 关于转换矩阵H，一般我们无法直接测得需要的待测量，所以会测量其他量计算出待测量。举个例子

激光测距仪测距：发射激光，根据返回用时计算距离。直接获得的物理量是 时间($Z_n$)，而我们想获得的 距离($X_n$)，于是有了转换矩阵 H($ \begin{bmatrix}\frac{2}{c}\end{bmatrix}$)

$$Z_n = \begin{bmatrix}\frac{2}{c}\end{bmatrix}X_n + V_n$$

整体介绍

预测

先验估计方程：

$$\hat{X}_k^- = A\hat{X}_{k-1} + Bu_{k-1}$$

先验误差协方差矩阵：

$$P_k^{-} = AP_{k-1}A^T + Q$$

矫正

卡尔曼增益：

$$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^-$$

变量说明

• $\hat{X}_k^-$：k时刻先验估计值——算出来的
• $\hat{X}_k$：最优估计值
• $\hat{X}_{k-1}$：k-1时刻最优估计值
• $u_k-1$：控制输入量
• $K_k$：卡尔曼增益
• $Z_k$：测量值——测出来的
• $W_{k-1}$：过程噪声
• $V_k$：测量噪声
• H：由状态量向测量量转换的矩阵

变量带个帽子一般是估计值；变量右上角有个”-“一般是先验值。

流程概述

1. 由上一时刻最优估计值 $\hat{X}_{k-1}$计算先验估计值 $\hat{X}_k^-$
2. 获取测量值 $Z_k$
3. 由上一时刻误差协方差矩阵 $P_{k-1}$计算先验误差协方差矩阵 $P_K^-$
4. 计算卡尔曼增益$K_k$
5. 计算最优估计值$\hat{X}_k$
6. 更新后验误差协方差矩阵 $P_k$
7. 不断更新迭代

公式说明与推导

先验估计方程

$$\hat{X}_k^- = A\hat{X}_{k-1} + Bu_{k-1}$$

​ 由上一时刻的最优估计值，根据运动模型推测这一时刻的状态值，即先验估计值。和传递方程类似。

后验估计方程

$$\hat{X}_k = \hat{X}_k^- + K_k( Z_k - H\hat{X}_k^-)$$

​ 想要得到最优估计值，概括来说是根据数据的可靠性将测量值和计算值加权叠加起来，得到的就是后验估计值，也就是滤波过后得到的这一时刻的最优估计值。根据数据融合原理得到公式：

$$\hat{X}_k = \hat{X}_k^- + G( H^-Z_k - \hat{X}_k^-)$$

但是我们经常看到的都是令 $G = K_kH$ 变换后的形式，即：

$$\hat{X}_k = \hat{X}_{k}^- + K_k ( Z_k - H\hat{X}_{k}^- )$$

其中 $K_k$∈[0, $H^-$]

• 当$K_k$ == 0 时，测量误差很大，信任计算出来的值，即 $\hat{X}_{k} == \hat{X}_{k}^-$;
• 当$K_k == H^-$时，无测量误差，信任测量值，即$\hat{X}_{k} == H^-Z_k$；

卡尔曼增益

$$K_k = \frac{P_k^- H^T}{HP_k^- H^T + R}$$

要计算卡尔曼增益，即求出使误差协方差最小的 $k_k$，在求出其关于$K_k$ 的表达式后再求导得到极值点。

状态空间方程：

$$\hat{X}_k^- = A\hat{X}_{k-1} + Bu_{k-1} + W_{k-1}$$ $$Z_k = HX_k + V_k$$

先后验误差协方差矩阵

$$P_k^{-} = AP_{k-1}A^T + Q$$ $$P_k = ( I - K_kH)P_k^-$$

应用举例

demo

kalman_filter

void filter(Matrix_t Z)
{
    count++;
    pre_angle1 += Z.matrix[0][0];
    pre_angle2 += Z.matrix[1][0];
    pre_angle3 = (pre_angle1 + pre_angle2)/2;

    _X = mul_matrix(F,X);
    _P = add_matrix(mul_matrix(mul_matrix(F, P), tran_matrix(F)), Q);
    K  = mul_matrix(mul_matrix(_P,tran_matrix(H)), inv_matrix(add_matrix(mul_matrix(mul_matrix(H,_P),tran_matrix(H)),R)));
    X  = add_matrix(_X,mul_matrix(K,sub_matrix(Z,mul_matrix(H,_X) ) ) );
    P  = mul_matrix(sub_matrix(get_I(2),mul_matrix(K,H)),_P); 

    fprintf(fp,"%d\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n",count, pre_angle1, pre_angle2, pre_angle3, X.matrix[0][0], X.matrix[1][0], Z.matrix[0][0], Z.matrix[1][0]);
    return;
}

执行一次filter( )就是进行了一次迭代。

这里使用fprintf将数据打印至excel文件，绘出波形。

当然，这里还用到了一些矩阵运算函数，我写在Matrix.h里面，下次想运算矩阵就直接调用啦，非常方便。不过更多的运算函数有待进一步的完善。

• add_matrix：矩阵加法
• sub_matrix：矩阵减法
• mul_matrix：矩阵乘法
• inv_matrix：矩阵求逆
• tran_matrix：矩阵转置

MPU6050滤波

状态变量

$$X = \begin{bmatrix} angle \\ bias \end{bmatrix}$$

​ 选择angle和bias作为状态变量，其中angle为角度，bias为陀螺仪的零漂。bias就是在传感器静止时也会有的输出的角速度值，这个只是不确定的，而且会变化。但是即使我们随便给它一个初值，最终也会随着迭代次数的增加收敛到真实值附近

状态转移方程

$$X^-_k = \begin{bmatrix} angle \\ bias \end{bmatrix} = \begin{bmatrix} 1 & -dt \\ 0 & 1 \\ \end{bmatrix} X_{k-1} + \begin{bmatrix} gyro*dt \\ 0 \end{bmatrix}$$

先验协方差矩阵

$$P^-_k = \begin{bmatrix} 1 & -dt \\ 0 & 1\\ \end{bmatrix} P^-_{k-1} \begin{bmatrix} 1 & 0 \\ -dt & 1 \\ \end{bmatrix} + \begin{bmatrix} Q_{angle} & 0 \\ 0 & Q_{bias} \\ \end{bmatrix}$$

测量方程

传入的只有滤波之前的角度值，故有

$$Z = HX + V \\ H = \begin{bmatrix}1 & 0\end{bmatrix}$$

这里的原始角度值是由IMU的三轴加速度值直接解算而来。

后面的几个方程就没什么变化了。

写在最后

​ 感谢大家耐心看完了本篇博客，欢迎大家分享；由于我自己也是初学者，对卡尔曼滤波的理解还不算深入，难免会有不完善之处，这篇文章作为一篇笔记由于大家分享，也欢迎大家批评指正。