基于加速度计与气压计的三阶卡尔曼滤波计算加速度、速度及高度

本文主要介绍了卡尔曼滤波器的使用原理，给出了matlab代码，并在STM32F407平台对卡尔曼滤波器进行了验证，传感器为MPU6050与DPS310，测试结果令人满意，速度与高度无累积误差。

---

系统状态方程

在开始讲卡尔曼滤波器之前需要先提一下状态方程。因为卡尔曼的计算公式是建立在状态方程上的，所以我们需要先写出系统的状态方程。离散状态方程为：

其中X(k)为当前状态，X(k+1)为下一时刻状态，Φ为转移矩阵，B为控制矩阵，u为控制量，Г为噪声矩阵，W为系统噪声，Y为输出量，H为输出矩阵，V为观测噪声。简单来说就是通过这一时刻已知的状态、控制量及系统噪声可以求出此刻的能观测到的输出以及下一时刻的状态。

那很么又是状态呢？对于我们要分析的系统来说，加速度、速度、以及高度就是系统的状态，也就是说公式中的X(k)就是包含加速度、速度、以及高度的向量。

同理

而状态转移矩阵Φ是表述下一时刻状态与此刻状态关系的矩阵，在本系统中我们能够非常清楚得列出他们的关系，假设我们采样周期T比较短，可以近似认为加速度a几乎不变（关于为什么a认为不变可参考卡尔曼滤波器阶次问题），则

将上面几个等式写成矩阵形式则为

由此我们可以得到转移矩阵Φ就是

对于我们要分析的系统，没有控制量，不考虑其他系统噪声的情况下，后面两项可以直接拿掉，状态方程简化为

状态方程第一个式子分析完了接下来分析下第二个式子。

我目前现有的传感器为MPU6050以及气压计DPS310，能够得到的物理量为加速度及气压，也就是说我能直接观测到的物理量是加速度和气压，那么状态方程的观测输出Y就是加速度和气压

那么如何从系统状态X(k)得到Y(k)呢？这个时候查阅网上资料发现在海拔较低且变化范围几百米以内时，气压与高度成线性关系，其系数为0.09，气压每变化ΔP时，高度变化0.09*ΔP，由此我们可以得到关系

其中P0为参考平面的气压，也就是h=0时的气压。然后我们就可以写成矩阵形式

得到H和V

我们得到系统的离散状态方程后，可以开始卡尔曼滤波了。

---

卡尔曼滤波

虽说叫卡尔曼滤波，但是它不止有滤波功能，还能对多个传感器得到的数据进行数据融合，因此应用非常广泛。这里就不细讲晦涩难懂的卡尔曼具体原理和推导了，只给出线性卡尔曼公式和每个公式的作用。

首先来感性得认识一下两个直接影响卡尔曼滤波效果的参数测量误差R和过程误差Q。测量误差R是反映传感器得到信息质量的优劣，传感器得到信号质量越差，则R应越大，从而有更强的滤波效果。虽然R越大滤波效果更强，但是响应速度会变慢，因此R不宜过大。一般可以通过传感器的数据手册直接得到其测量误差，也可以一个个值试，直到得到想要的滤波效果。而过程误差Q反映的是在测量过程中受到别的环境因素影响的大小，如气压计易受到风、温度的干扰之类的，当Q为0时，得到的滤波效果会非常平滑，但是会存在累积误差之类的缺点，当Q较大时，滤波效果会变差，一般Q取一个较小值比较合适，比如0.0001。

一次卡尔曼滤波可分为以下五个过程

1. 通过上一次状态的最优估计X(k-1)得到本次状态的预测
2. 计算本次协方差矩阵预测
3. 计算滤波增益矩阵K(k)
4. 根据实际观测输出Y(k)、增益矩阵K(k)对本次预测的状态进行修正得到本次状态最优估计X(k)
5. 更新协方差矩阵P(k)

第一步：计算本次状态预测

本次状态预测就是将上次最优估计得到的状态代入状态方程即可得到。

第二步：计算本次协方差矩阵预测

我们的状态有h、v、a三个变量，故协方差矩阵P为三阶矩阵，P(k-1)为上次协方差矩阵，初始协方差矩阵P(0)可设为对角阵，对角上每个p值为三个变量对应的初始协方差，一般取1-10，初始协方差矩阵对后面没有影响。本系统中，Г为单位矩阵，Q为3阶对角阵，对角上的每个q值为三个变量对应的过程误差。

第三步：计算滤波增益矩阵

由上一步得到的、输出矩阵H以及初始化时设定好的观测噪声矩阵R计算得到本次滤波增益矩阵K(k)。

第四步：计算本次状态最优估计

对第一步得到预测状态进行修正得到本次最优估计。其中Y(k)为本次传感器得到的实际测量值。

第五步：更新协方差矩阵

根据前几步得到的增益矩阵K与协方差估计矩阵得到本次协方差矩阵。

---

Matlab仿真

DataUp.mat中包含MPU6050采集到的三轴加速度以及气压计DPS310采集到的气压，数据从电梯轿厢中测得，由于电梯开门后轿厢内气压值有变化，因此开门时气压值有些变化，造成速度计算值有些偏差。

clear;
load('DataUp.mat');
len = length(z);
% 减去重力加速度后的加速度
a = (z-1)*9.8;

% 参数设置
sam_frq = 1000;
T = 1/sam_frq;
k = 0.09;%压高系数

% 观测误差R、过程误差Q
R = 0.5*eye(2);
R(1,1) = 150;    
R(2,2) = 0.5;
Q = 0.0001*eye(3);
Q(2,2) = 0.00001;

% 状态方程矩阵
F = [1,T,0.5*T*T;0,1,T;0,0,1];
H = [-1/k,0,0;0,0,1];
V = [pre(1);0];

% 数据初始化
Xkf = zeros(3,len);
Z = zeros(2,len);
Z(1,:) = pre;
Z(2,:) = a;
Xkf(:,1) = [0;0;0];
P0 = eye(3);
P0(1,1) = 10;

% 卡尔曼滤波
for i = 2:len
    Xn = F*Xkf(:,i-1);
    P1 = F*P0*F'+ Q;
    K = (P1*H')/(H*P1*H'+R);
    Xkf(:,i) = Xn + K*(Z(:,i)-H*Xn-V);
    P0 = (eye(3)-K*H)*P1;
end

% 显示图像
figure;
plot((pre(1)-pre)*0.09);
hold on;
plot(Xkf(1,:));
hold on;
plot(Xkf(2,:));
hold on;
plot(Xkf(3,:));
hold on;
plot(a);

以下是仿真结果

气压计滤波效果：

速度计算效果：

加速度滤波效果：

STM32F407平台验证

实际传感器都有线性偏差，尤其是MPU6050的线性偏差也就是三个轴加速度的真实值a=kx+b，通过最小二乘法确定k和b。获得加速度计以多个姿态静置时(确保只受到重力加速度)的加速度作为拟合数据后，按照Matlab中的lsqcurvefit，非线性拟合中给出拟合方法进行拟合得到最终线性补偿系数k、b。

在实际应用中系数k对最终结果影响不大，但我们仍然需要得到系数b，因为加速度计的固定偏差会影响误差的累计。b的获取可通过静置时加速度相对于重力加速的的偏移得到。

先读出MPU6050的三轴加速度原始值，即三个short类型数据。经过处理后得到除重力加速度的垂直方向加速度。MPU6050水平放置，则z轴加速度可近似为垂直方向的加速度。

//加速度补偿值计算
for(int i=0; i<100; i++){
	MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
	b = Acc_Comp(aacz/16384.0*G - G);
	delay_ms(5);
}

其中补偿值计算方式为

/**
  * @func	加速度计补偿值计算
  * @param	a			减去重力加速度后的加速度
  * 
  * @ret	补偿值
  * @use	循环使用该函数20次以上才能得到比较精确的补偿值
  */
float Acc_Comp(float a)
{
	static float b = 0;
	b = b*0.95 + a*0.05;
	return -b;
}

主循环

 while(1)
{
    // 读取加速度
    MPU_Get_Accelerometer(&aacx,&aacy,&aacz);	

    // 计算加速度
    accexx = aacx/16384.0*G;
    acceyy = aacy/16384.0*G;
    accezz = aacz/16384.0*G - G + b;
	
    // 读取气压
    Dps301_read_press(&pressure, cal_coe);
	
    // 卡尔曼滤波
    Kalman_Fil_Calc(&KF, accezz, pressure);
		
    // 获得滤波后的高度、速度和加速度
    h = KF.X[0][0];
    v = KF.X[1][0];
    a = KF.X[2][0];

    LED0=!LED0;
    delay_ms(10);
} 	

最终通过jscope观察h、v、a的波形输出

完整matlab代码及keil工程

卡尔曼滤波器计算加速度、速度和高度的Matlab仿真和STM32验证

关于评论中提到的速度为0的BUG解决方法是在“kalman_rank2.h”文件中Period的宏定义前加个强制类型转换即可，新上传的资源已修复该bug

另外在实际应用中采样频率对测量误差R的影响很大，修改完采样频率后需要修改卡尔曼滤波器初始化时传入的参数pre_r和acc_r，具体值以实际效果好为准，不用考虑范围，如采样频率为5Hz时，可修改为pre_r=0.01，acc_r=0.0003