卡尔曼滤波算法及其应用

卡尔曼滤波算法及其应用

本文是在学习 卡尔曼滤波器的过程中所做的一些笔记，好记心不如烂笔头！本人正在从事和无人驾驶有关的工作，个人公众号为：Kevin的学习站，也建立公众号交流群，欢迎加我QQ：643470489，一起交流学习哟！

卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器， 卡尔曼滤波在技术领域有许多的应用，常见的有飞机及太空船的导引、导航及控制、信号处理、机器人运动规划及控制等，也包括在轨迹最佳化。

卡尔曼滤波能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会比只以单一测量量为基础的估计方式要准。

1、卡尔曼滤波器系统模型介绍

1.1、系统离线型状态方程

卡尔曼滤波器适用于线性系统， 由 k-1 时刻到 k 时刻，系统状态预测方程为：

x(k) = A · x(k-1) + B · u(k) + w(k)  

系统状态观测方程 ：

z(k) = H · x(k) + y(k)

1.2、变量说明

x(k) —— k 时刻系统的状态
u(k) —— 控制量
w(k) —— 符合高斯分布的过程噪声，其协方差在下文中为 Q
z(k) —— k 时刻系统的观测值
y(k) —— 符合高斯分布的测量噪声，其协方差在下文中为 R
A——状态转移矩阵
B——输入增益矩阵
H——量测矩阵
A、 B、 H —— 系统参数，多输入多输出时为矩阵，单输入单输出时就是几个常数

两个噪声 通常不用w(k)和 y(k)表示，而是用到他们的协方差 Q 和 R表示。

2、卡尔曼滤波器

卡尔曼估计实际由两个过程组成：预测与校正。

预测阶段： 滤波器使用上一状态的估计，做出对当前状态的预测。

校正阶段： 滤波器利用对当前状态的观测值修正在预测阶段获得的预测值，以获得一个更接进真实值的新估计值。

2.1、卡尔曼滤波计算公式

预测：

x(k|k-1) = A·x(k-1|k-1) + B·u(k)

P(k|k-1) = A·P(k-1|k-1)·A^T + Q

校正：

z(k) = H·x(k) + y(k)

K(k) = P(k|k-1)·H^T·(H·P(k|k-1)·H^T + R)^(-1)

x(k|k) = x(k|k-1) + K(k)·(z(k) - H·x(k|k-1))

更新协方差估计：

P(k|k) = (I - K(k) · H) · P(k|k-1)

2.2、推导过程简述

工程中用到的卡尔曼滤波是一个迭代的过程，每得到一个新的观测值迭代一次，来回来去地更新两个东西： “系统状态”(x)和“误差协方差”§。由于每次迭代只用到上一次迭代的结果和新的测量值，这样滤波对计算资源的占用是很小的。

每一次迭代，两个步骤：预测和修正。预测是根据前一时刻迭代的结果，即 x(k-1|k-1)和 P(k-1|k-1)，来预测这一时刻的系统状态和误差协方差，得到 x(k|k-1)和 P(k|k-1)：

x(k|k-1) = A · x(k-1|k-1) + B · u(k)

P(k|k-1) = A · P(k-1|k-1) · AT + Q

注：式用到的 A、 B、 H、 Q、 R 就是从前面的状态/观测方程中得到的。

然后计算卡尔曼增益 K(k)，和这一次的实际测量结果 z(k)一起，用于修正系统状态x(k|k-1)及误差协方差 P(k|k-1)，得到最新的 x(k|k)和 P(k|k)：

至此这次迭代就算结束了， x(k|k)就是我们要的滤波后的值，它和 P(k|k)将会作为x(k-1|k-1)和 P(k-1|k-1)用在下一时刻的迭代里。

3、用卡尔曼滤波器在测量温度中的应用

此时控制量是没有的，即：

 B · u(k) ≡ 0

将参数 A 和 H 简单地取 1。此时滤波器的预测方程简化为：

 ① x(k|k-1) = x(k-1|k-1)
                    
 ② P(k|k-1) = P(k-1|k-1) + Q

修正方程变成：

 ③ K(k) = P(k|k-1) / (P(k|k-1) + R)

 ④ x(k|k) = x(k|k-1) + K(k) · (z(k) - x(k|k-1))

 ⑤ P(k|k) = (1 - K(k)) · P(k|k-1)

上式中 ①对②、 ③无影响，于是④可以写成：

x(k|k) = x(k-1|k-1) + K(k) · (z(k) - x(k-1|k-1))

在程序中，该式写起来更简单：

x = x + K · (新观测值 - x);

观察②，对这一时刻的预测值不就是上一时刻的修正值+Q 嘛，不妨把它合并到上一次迭代中，即⑤改写成：

 P(k+1|k) = (1 - K(k)) · P(k|k-1) + Q

这一时刻的 P(k+1|k)，会作为下一时刻的 P(k|k-1)，刚好是③需要的。于是整个滤波过程只用这三个式子来迭代即可：

K(k) = P(k|k-1) / (P(k|k-1) + R)

x(k|k) = x(k-1|k-1) + K(k) · (z(k) - x(k-1|k-1))

P(k+1|k) = (1 - K(k)) · P(k|k-1) + Q

4、卡尔曼滤波算法的实现

#include "filter.h"
//变量声明
float K1 =0.02;
float angle, angle_dot; // 角度， 角速度
float Q_angle=0.001; // 陀螺仪噪声的协方差
float Q_gyro=0.003; // 陀螺仪漂移噪声的协方差
float R_angle=0.5; // 加速度计测量噪声的协方差
float dt=0.005; // 积分时间， dt 为滤波器采样时间（秒）
char C_0 = 1; // H 矩阵的一个数
float Q_bias, Angle_err; // Q_bias 为陀螺仪漂移
float PCt_0, PCt_1, E; //中间变量
float K_0, K_1, t_0, t_1; //K 是卡尔曼增益， t 是中间变量
float Pdot[4] ={0,0,0,0}; //计算 P 矩阵的中间变量
float PP[2][2] = { { 1, 0 },{ 0, 1 } }; //公式中 P 矩阵， X 的协方差

//函数功能：简易卡尔曼滤波
//入口参数：加速度、角速度
//返回 值：无
// X(k|k-1)=A X(k-1|k-1)+B U(k) ………(1)先验估计
// P(k|k-1)=A P(k-1|k-1) A’+Q ………(2)协方差矩阵的预测
// Kg(k)= P(k|k-1) H’ / (HP(k|k-1) H’ + R) ………(3)计算卡尔曼增益
// X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ………(4)通过卡尔曼增益进行修正
// P(k|k)=（I-Kg(k) H） P(k|k-1) ………(5)更新协方差阵

void Kalman_Filter(float Accel,float Gyro) //Gyro 陀螺仪的量测值， Accel 加速度计的角度计算值
{
    angle+=(Gyro - Q_bias) * dt; //先验估计 对应第一个公式
    Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
    Pdot[1]=-PP[1][1];
    Pdot[2]=-PP[1][1];
    Pdot[3]=Q_gyro;
    PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
    PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
    PP[1][0] += Pdot[2] * dt;
    PP[1][1] += Pdot[3] * dt;
    //以上 8 行对应第二个方程
    //计算卡尔曼增益 K_0,K_1
    Angle_err = Accel - angle; //zk-先验估计
    PCt_0 = C_0 * PP[0][0]; //矩阵乘法的中间变量
    PCt_1 = C_0 * PP[1][0]; //C_0=1
    E = R_angle + C_0 * PCt_0; //分母
    K_0 = PCt_0 / E; //卡尔曼增益，两个，一个是 Angle 的，一个是 Q_bias 的
    K_1 = PCt_1 / E;
    //以上 6 行对应公式 3
    //对 PK 进行更新
    t_0 = PCt_0; //矩阵计算中间变量，相当于 a
    t_1 = C_0 * PP[0][1]; //矩阵计算中间变量，相当于 b
    PP[0][0] -= K_0 * t_0; //后验估计误差协方差
    PP[0][1] -= K_0 * t_1;
    PP[1][0] -= K_1 * t_0;
    PP[1][1] -= K_1 * t_1;
    //以上 6 行对应公式 5
    //通过卡尔曼增益修正当前角度、陀螺仪零点、当前角速度
    angle += K_0 * Angle_err; //后验估计
    Q_bias += K_1 * Angle_err; //后验估计
    angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
    //对应第 4 个方程
}

欢迎关注公众号：Kevin的学习站，创作不易，但您的点赞、关注、收藏就是对我最大的鼓励！