概述

在实际项目中，常常需要对数据进行滤波，这里分享卡尔曼滤波算法应用。

正文

卡尔曼滤波（KF）属于线性滤波器，使用均值和方差描述系统状态，将多个符合高斯分布的不确定信息源进行数据融合的一种最优状态估计算法。

使用前提：

• 线性：系统状态方程为线性（比例性、齐次性）的系统，非线性则需要使用扩展卡尔曼滤波（EKF）。

• KF和EKF最大的区别在于：EKF的$F_k$和$H_k$都是使用雅各比矩阵表示

• 高斯分布：系统噪声服从高斯分布

  • 传感器噪声

  • ADC采样噪声

预测方程：

${\hat{X}}_k=F_k{\hat{X}}_{k-1}+B_k{\vec{U}}_k$

$P_k=F_k{P}_{k-1}{F}_k^T+Q_k$

更新方程：

$K^{\prime }=\frac{P_k{H}_k^T}{H_k{P}_k{H}_k^T+R_k}$

$\widehat{X_k^{\prime }}={\hat{X}}_k+K^{\prime }({\vec{Z}}_k-H_k{\hat{X}}_k)$

${P_k}^{\prime }=P_k-K^{\prime }{H}_k{P}_k$

• ${\hat{X}}_k$为$k$时刻系统最优的状态向量

• $F_k$为状态转移矩阵，表示用$k-1$时刻的状态向量估算$k$时刻的状态向量

• $B_k$为输入控制矩阵，代表着控制向量${\vec{U}}_k$和状态向量${\hat{X}}_k$的关系

• ${\vec{U}}_k$代表着控制向量，如加速度、角加速度等

• $P_k$为状态向量的协方差矩阵，代表着状态向量每个元素之间的关系

• $Q_k$为预测状态的高斯噪声协方差矩阵，它用来衡量模型的准确度，模型越准确其值越小

• ${\vec{Z}}_k$为$k$时刻测量值的状态向量

• $H_k$为转换矩阵，它将状态向量${\hat{X}}_k$映射到测量值所在的向量空间${\vec{Z}}_k$

• $R_k$为测量值的高斯噪声协方差矩阵，代表着传感器的测量误差

一阶卡尔曼滤波

(1)预测状态方程：

在一阶系统下，$F_k$的值为1，无外部控制向量${\vec{U}}_k$的作用。所以，代入公式${\hat{X}}_k=F_k{\hat{X}}_{k-1}+B_k{\vec{U}}_k$得出：

${\hat{X}}_k={\hat{X}}_{k-1}$

(2)预测状态协方差方程：

在一阶系统下，$F_k$的值为1，所以转置矩阵$F_k^T$的值也为1。所以，代入公式$P_k=F_k{P}_{k-1}{F}_k^T+Q_k$得出：

$P_k=P_{k-1}+Q_k$

(3)卡尔曼增益方程：

在一阶系统下，$H_k$的值为1。所以，代入公式$K^{\prime }=\frac{P_k{H}_k^T}{H_k{P}_k{H}_k^T+R_k}$得出：

$K^{\prime }=\frac{P_k}{P_k+R_k}$

(4)更新最优状态方程：

在一阶系统下，$H_k$的值为1。所以，代入公式$\widehat{X_k^{\prime }}={\hat{X}}_k+K^{\prime }({\vec{Z}}_k-H_k{\hat{X}}_k)$得出：

$\widehat{X_k^{\prime }}={\hat{X}}_k+K^{\prime }({\vec{Z}}_k-{\hat{X}}_k)$

(5)更新状态协方差方程：

在一阶系统下，$H_k$的值为1。所以，代入公式${P_k}^{\prime }=P_k-K^{\prime }{H}_k{P}_k$得出：

${P_k}^{\prime }=P_k-K^{\prime }{P}_k$

C代码实现

typedef struct{float last_P;    float Q;         float R;         float out;float now_P;float Kg;
}kalman_t;kalman_t my_kalman;
float kalman_filter_init(void)
{my_kalman.last_P = 1.0;my_kalman.Q = 0;my_kalman.R = 0.01;my_kalman.out = 0;my_kalman.now_P = 0;my_kalman.Kg = 0;
}float kalman_filter_calc(kalman_t *kfp, float input)
{kfp->now_P = kfp->last_P + kfp->Q;kfp->Kg = kfp->now_P / (kfp->now_P + kfp->R);kfp->out += kfp->Kg * (input - kfp->out);kfp->last_P = (1 - kfp->Kg) * kfp->now_P;return kfp->out;
}

实验效果

二阶卡尔曼滤波

以经典卡尔曼滤波模型：加速度计、陀螺仪进行角度数据融合为例。

(1)预测状态方程：

首先，根据系统建立系统方程：

$angl{e}_k=angl{e}_{k-1}-Q\_bia{s}_{k-1}\ast \Delta t+Gyr{o}_k\ast \Delta t+\frac{1}{2}\ast (Gyr{o}_k-Gyr{o}_{k-1})\ast {{\Delta }_t}^2$

$Q\_bia{s}_k=Q\_bia{s}_{k-1}$

用矩阵的方式表示为：

$\left|\begin{array}{c}angl{e}_k\\ Q\_bia{s}_k\end{array}\right|=\left|\begin{array}{cc}1& -\Delta t\\ 0& 1\end{array}\right|\left|\begin{array}{c}angl{e}_{k-1}\\ Q\_bia{s}_{k-1}\end{array}\right|+\left|\begin{array}{c}\Delta t\\ 0\end{array}\right|Gyr{o}_k+\left|\begin{array}{c}\frac{1}{2}\ast {{\Delta }_t}^2\\ 0\end{array}\right|(Gyr{o}_k-Gyr{o}_{k-1})$

• $angle$为角度，如pitch、roll、yaw

• $Q\_bias$为陀螺仪零漂，固定值

• 由于${{\Delta }_t}^2$非常小，所以忽略不计，所以$B_k$为0

• $F_k=\left|\begin{array}{cc}1& -\Delta t\\ 0& 1\end{array}\right|$

所以，预测状态方程的C代码如下：

angle += (gyro - Q_bias) * dt;//预测角度等于上次最优角度加上//角速度减去零漂后的积分角度

(2)预测状态协方差方程：

由于高斯分布中的每个数据点的协方差可以用$Cov(x)=\Sigma$表示，所以高斯噪声协方差矩阵$Q_k=\left|\begin{array}{cc}Cov(angle,angle)& Cov(Q\_bias,angle)\\ Cov(angle,Q\_bias)& Cov(Q\_bias,Q\_bias)\end{array}\right|$

因为加速度计和陀螺仪噪声相互独立，所以：

$Q_k=\left|\begin{array}{cc}Cov(angle,angle)& 0\\ 0& Cov(Q_{b}ias,Q_{b}ias)\end{array}\right|$

假设预测状态协方差矩阵$P_k=\left|\begin{array}{cc}{a}_k& b_k\\ c_k& d_k\end{array}\right|$

代入预测状态协方差方程得出：

$P_k=\left|\begin{array}{cc}{a}_{k-1}-(c_{k-1}+b_{k-1})\ast \Delta t+{\Delta t}^2& b_{k-1}-d_{k-1}\ast \Delta t\\ c_{k-1}-d_{k-1}\ast \Delta t& d_{k-1}\end{array}\right|+\left|\begin{array}{cc}Cov(angle,angle)& 0\\ 0& Cov(Q\_bias,Q\_bias)\end{array}\right|$

• 由于${{\Delta }_t}^2$非常小，所以忽略不计

所以，预测状态协方差方程的C代码如下：

PP[0][0] = PP[0][0] + Q_angle - (PP[0][1] - PP[1][0])*dt;
PP[0][1] = PP[0][1] - PP[1][1]*dt;
PP[1][0] = PP[1][0] - PP[1][1]*dt;
PP[1][1] = PP[1][0] + Q_gyro;

(3)卡尔曼增益方程：

首先建立测量方程$Z_k=H_k{X}_k+V_k$

• $Z_k$为$k$时刻实际观测量

• $X_k$为$k$时刻状态量

• $V_k$为$k$时刻的噪声

由于传感器数据自带噪声，所以$V_k=0$

所以测量方程可以用矩阵表示为：

$Z_k=\left|\begin{array}{cc}1& 0\end{array}\right|\left|\begin{array}{c}{X}_k\\ 0\end{array}\right|$

所以转换矩阵$H_k=\left|\begin{array}{cc}1& 0\end{array}\right|$

代入卡尔曼增益方程得出：

$K^{\prime }=\frac{\left|\begin{array}{cc}a& b\\ c& d\end{array}\right|\left|\begin{array}{c}1\\ 0\end{array}\right|}{|\begin{array}{cc}1& 0\end{array}|\left|\begin{array}{cc}a& b\\ c& d\end{array}\right|\left|\begin{array}{c}1\\ 0\end{array}\right|+R\_angle}=\frac{a+c}{a+R\_angle}$

所以卡尔曼增益方程的C代码如下：

K_0 = PP[0][0] / (PP[0][0] + R_angle);    //angle增益
K_1 = PP[1][0] / (PP[0][0] + R_angle);    //Q_bias增益

(4)更新最优状态方程：

代入更新最优状态方程得出：

$\left|\begin{array}{c}angle\\ Q\_bias\end{array}\right|=\left|\begin{array}{c}angle\\ Q\_bias\end{array}\right|+\left|\begin{array}{c}{K}_0\\ K_1\end{array}\right|(angl{e}_z-\left|\begin{array}{cc}1& 0\end{array}\right|\left|\begin{array}{c}angle\\ Q\_bias\end{array}\right|)$

所以更新最优状态方程的C代码如下：

Q_bias += K_1 * (angle_now - angle);
angle += K_0 * (angle_now - angle);

(5)更新状态协方差方程

代入更新状态协方差方程得出：

$\left|\begin{array}{cc}{a_k}^{\prime }& {b_k}^{\prime }\\ {c_k}^{\prime }& {d_k}^{\prime }\end{array}\right|=(\left|\begin{array}{cc}1& 0\\ 0& 1\end{array}\right|-\left|\begin{array}{c}{K}_0\\ K_1\end{array}\right|\left|\begin{array}{cc}1& 0\end{array}\right|)\ast \left|\begin{array}{cc}{a}_k& b_k\\ c_k& d_k\end{array}\right|$

所以更新状态协方差的C代码如下：

PP[0][0] = PP[0][0] - K_0 * PP[0][0];
PP[0][1] = PP[0][1] - K_0 * PP[0][1];
PP[1][0] = PP[1][0] - K_1 * PP[0][0];
PP[1][1] = PP[1][1] - K_1 * PP[0][1];

整理完整代码

static float Q_angle = 0.001;		//角度噪声的协方差
static float Q_gyro  = 0.003;		//角速度噪声的协方差  
static float R_angle = 0.5;		    //加速度计测量噪声的协方差
static float dt      = 0.01;		//采样周期即计算任务周期10msfloat kalman_acc_gyro(float angle_now, float gyro)
{static float Q_bias;static float K_0,K_1;   static float PP[2][2] = {{1, 0}, {0, 1}};static float angle;angle += (gyro - Q_bias) * dt;      PP[0][0] += Q_angle - (PP[0][1] - PP[1][0])*dt;PP[0][1] -= PP[1][1]*dt;PP[1][0] -= PP[1][1]*dt;PP[1][1] += Q_gyro;K_0 = PP[0][0] / (PP[0][0] + R_angle);    K_1 = PP[1][0] / (PP[0][0] + R_angle);Q_bias += K_1 * (angle_now - angle);angle += K_0 * (angle_now - angle);PP[0][0] -= K_0 * PP[0][0];PP[0][1] -= K_0 * PP[0][1];PP[1][0] -= K_1 * PP[0][0];PP[1][1] -= K_1 * PP[0][1];return angle;
}void test(float acc_x, float acc_y, float acc_z,float gyro_x, float gyro_y, float gyro_z)
{float roll = (atan(acc_x/acc_z))*180/3.14159;kalman_acc_gyro(roll, gyro_y);
}

实验效果

总结

这样就完成了卡尔曼滤波的简单分析和基础应用，实际项目中不同系统的传递函数不同，所以需要进行调整设计，但是5大核心公式都不会变。扩展卡尔曼其实也是利用线性去模拟非线性，具体的应用大家有兴趣可以自行查文献实验，这里不做分析介绍。水平有限，这里只做自己的理解分析，有错误的地方，请大佬们不吝赐教！