本文主要介绍了卡尔曼滤波器的使用原理，给出了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

以下计算方法适合于GPS L1 NAV星历 、BDII代 D1星历，其中：

是地心引力常数，是地球自转角速率，其值的大小参考对应的ICD文档。

1、广播星历参数表

参考时间：

轨道长轴平方根：

偏心率：

近地点幅角：

卫星平均运动速率与计算值之差：

参考时刻平近点角：

参考时刻升交点赤经：

升交点赤经变化率：

参考时刻轨道倾角：

轨道倾角变化率：

轨道改正项参数：

2、计算卫星在ECEF坐标系下的位置坐标

（1）计算：

（2）计算卫星的平均角速率n：

（3）计算平近点角：

（4）计算偏近点角（迭代计算）：

（5）计算真近点角: 

（6）计算升交点角距: 

（7）计算摄动校正项: 

（8）计算摄动校正后的升交点角距：

（9）计算摄动校正后的矢径长度：

（10）计算摄动校正后的轨道倾角：

（11）计算卫星在轨道面上的位置：

（12）计算升交点赤经: 

（13）计算卫星在ECEF坐标系下的位置：

3、计算卫星在ECEF坐标系下的速度

（1）计算平近点角对时间的一阶导数：

（2）计算偏近点角对时间的一阶导数：

（3）计算真近点角的一阶导数: 

（4）计算升交点角距的一阶导数:

（5）计算摄动校正项的一阶导数: 

（6）计算摄动校正后的升交点角距的一阶导数：

（7）计算摄动校正后的矢径长度的一阶导数：

（8）计算摄动校正后的轨道倾角的一阶导数：

（9）计算卫星在轨道面上的速度:

（10）计算升交点赤经的一阶导数:          

（11）计算卫星在ECEF坐标系下的速度

4、计算卫星在ECEF坐标系下的加速度

（1）计算平近点角对时间的二阶导数：

（2）计算偏近点角对时间的二阶导数：

（3）计算真近点角的二阶导数: 

（4）计算升交点角距的二阶导数: 

（5）计算摄动校正项的二阶导数:: 

（6）计算摄动校正后的升交点角距的二阶导数：

（7）计算摄动校正后的矢径长度的二阶导数：

（8）计算摄动校正后的轨道倾角的二阶导数：

（9）计算卫星在轨道面上的加速度：

（10）计算升交点赤经的二阶导数: 

（11）计算卫星在ECEF坐标系下的加速度

5、计算卫星在ECEF坐标系下的加加速度

注意！！！本文转载自参考文献[2]。

1 各方向初始重力分量

如果芯片水平静置，X、Y方向的重力分量为0g，而Z轴方向的重力分量为g。如下图所示，X=0；Y=0；Z=g。图1 芯片水平静置

2 各方向重力分量计算

图2 各轴分别与水平线、重力加速度的夹角
图3 重力加速度在各轴的分量

step1:
各边与水平方向有一些夹角，则其图像如图2所示：

• X轴方向的加速度大小为Ax，其与水平线的夹角为α1，与重力加速度g的夹角为α
• Y轴方向的加速度大小为Ay，其与水平线的夹角为β1，与重力加速度g的夹角为β
• Z轴方向的加速度大小为Az，其与水平线的夹角为γ1，与重力加速度g的夹角为γ

step2:
基于图2中的夹角概念，它们的关系为：

• α = 90度- α1
• β = 90度- β1
• γ = 90度- γ1

step3:
如图3，g在各轴方向上的分量：

• Ax = gcosα
• Ay = gcosβ
• Az = gcosγ

step4:
将step2中数据代入step3得：

• Ax = gcosα = gcos(90度- α1) = gsinα1
• Ay = gsinβ1
• Az = gsinγ1

关于奇偶变换[1]
比如sin(x+nπ/2)，奇偶指的是n。
当n为偶数时候，三角函数名不变，还是sin 符号看象限是指把x 当做锐角然后算出sin(x+nπ/2)的值，看他的正负，这个值是正的，那么最后变换完的结果就是正的，这个值是负的，那么就是负的。
当n为奇数的时候，三角函数名改成另一个，这里就是cos。符号看象限同理。

step5:
如图3，由直角三角形的勾股定理得：

• Ax * Ax + gcosα1 * gcosα1 = g * g
• Ay * Ay + gcosβ1 * gcosβ1 = g * g
• Az * Az + gcosγ1 * gcosγ1 = g * g

step6:
如图3（详细版）、4所示，各垂直虚线的大小为（其中sqrt表示开根号）：

• gcosα1 = sqrt ( g * g - Ax * Ax )
• gcosβ1 = sqrt ( g * g - Ay * Ay )
• gcosγ1 = sqrt ( g * g - Az * Az )
  
  图4 重力加速度g在各轴上的分量
  
  图5 重力加速度g 作为立方体的对角线

step7:
如图5，在立体几何中，g相当于立方体的对角线，Ax、Ay、Az相当于三条边：
Ay * Ay + Az * Az = |虚线| * |虚线|
又
|虚线| * |虚线| + Ax * Ax = g * g
所以（根据勾股定理）：
Ax * Ax + Ay * Ay + Az * Az = g * g

step8:
如图4所示，(以X轴为例) sinα1 = Ax / g， cosα1 = sqrt(g * g - Ax * Ax) / g，

• tanα1 = (Ax / g) / [sqrt(g * g - Ax * Ax) / g] = Ax / sqrt(g * g - Ax * Ax) = Ax / sqrt(Ay * Ay + Az * Az)
• tanβ1 = Ay / sqrt(Ax * Ax + Az * Az)
• tanγ1 = Az / sqrt(Ax * Ax + Ay * Ay)

step9:
最后得出ADXL345加速度传感器，各轴加速度值与角速度值(弧度)的关系为：

• tanα1 = Ax / sqrt(Ay * Ay + Az * Az)
• tanβ1 = Ay / sqrt(Ax * Ax + Az * Az)
• tanγ1 = Az / sqrt(Ax * Ax + Ay * Ay)

其中 α1 、β1 、γ1分别是X、Y、Z轴和水平线的弧度值(反三角函数计算的值是弧度)，Ax 、Ay、Az是三个轴上的加速度值。

step10：
那么弧度值分别为：

• α1 = arctan(Ax / sqrt(Ay * Ay + Az * Az))
• β1 = arctan(Ay / sqrt(Ax * Ax + Az * Az))
• γ1 = arctan(Az / sqrt(Ax * Ax + Ay * Ay))

step11:
接下来就得使用数据公式：弧度= θπR/180 。 这样算得θ = 弧度 * 180/πR，其中R取1。最后得到的各轴的角度值分别为：

• θx = α1 * 180/π = [arctan(Ax / sqrt(Ay*Ay + Az * Az))] * 180/π
• θy = β1 * 180/π = [arctan(Ay / sqrt(Ax*Ax + Az * Az))] * 180/π
• θz = γ1 * 180/π = [arctan(Az / sqrt(Ax*Ax + Ay * Ay))] * 180/π

参考文献

[1] 数学三角函数中的“奇变偶不变，符号看象限”怎么理解
[2]三轴加速度传感器角度值转换原理 - CSDN博客