嵌入式开发-卡尔曼滤波算法及应用详解

先看一个对理解卡尔曼滤波能起到理解作用的的笑话:

一片绿油油的草地上有一条曲折的小径,通向一棵大树.一个要求被提出:从起点沿着小径走到树下.

“很简单.” A说,于是他丝毫不差地沿着小径走到了树下.

现在，难度被增加了：蒙上眼。

“也不难，我当过特种兵。” B说，于是他歪歪扭扭地走到了树旁。“唉，好久不练，生疏了。” （只凭自己的预测能力）

“看我的，我有 DIY 的 GPS！” C说，于是他像个醉汉似地歪歪扭扭的走到了树旁。“唉，这个 GPS 没做好，漂移太大。”（只依靠外界有很大噪声的测量）

“我来试试。” 旁边一也当过特种兵的拿过 GPS, 蒙上眼，居然沿着小径很顺滑的走到了树下。（自己能预测+测量结果的反馈）

“这么厉害！你是什么人?”

“卡尔曼 ! ”

“卡尔曼？！你就是卡尔曼？”众人大吃一惊。

“我是说这个 GPS 卡而慢。”

这个小笑话很有意思的指出了卡尔曼滤波的核心，预测+测量反馈，记住这种思想。

在介绍卡尔曼滤波前，简单说明几个在学卡尔曼过程中要用到的概念。即什么是协方差，它有什么含义，以及什么叫最小均方误差估计，什么是多元高斯分布。如果对这些有了了解，可以跳过，直接到下面的分割线。

均方误差：它是"误差"的平方的期望值（误差就是每个估计值与真实值的差），也就是多个样本的时候，均方误差等于每个样本的误差平方再乘以该样本出现的概率的和。

方差：方差是描述随机变量的离散程度，是变量离期望值的距离。

注意两者概念上稍有差别，当你的样本期望值就是真实值时，两者又完全相同。最小均方误差估计就是指估计参数时要使得估计出来的模型和真实值之间的误差平方期望值最小。

两个实变量之间的协方差：

它表示的两个变量之间的总体误差，当Y=X的时候就是方差。下面说说我对协方差的通俗理解，先抛去公式中的期望不谈，即假设样本X,Y发生的概率就是1，那么协方差的公式就变成了：

这就是两个东西相乘，马上联想到数值图像里的相关计算。如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。如果两个变量的变化趋势相反，即其中一个大于自身的期望值，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。协方差矩阵只不过就是元素多了组成了矩阵，其中协方差矩阵的对角线就是方差，具体公式形式请见自行百度搜索。

卡尔曼滤波简介：

卡尔曼滤波器（Kalman filter）是由卡尔曼（Kalman）在1960年提出的一种用于时变线性系统的递归滤波器。

卡尔曼滤波一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

卡尔曼滤波对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。因此，自从卡尔曼滤波理论问世以来，在通信系统、电力系统、航空航天、环境污染控制、工业控制、雷达信号处理等许多部门都得到了应用，取得了许多成功应用的成果。

卡尔曼滤波性质：

卡尔曼滤波是一个算法，它适用于线性、离散和有限维系统。每一个有外部变量的自回归移动平均系统(ARMAX)或可用有理传递函数表示的系统都可以转换成用状态空间表示的系统，从而能用卡尔曼滤波进行计算。

卡尔曼增益是在递归方程计算中动态变化的，目的是使系统估计值最优，这表明了卡尔曼滤波具有一定的自适应性。

当观测数据和状态联合服从高斯分布时用卡尔曼递归公式计算得到的是高斯随机变量的条件均值和条件方差，从而卡尔曼滤波公式给出了计算状态的条件概率密度的更新过程线性最小方差估计，也就是最小方差估计。

卡尔曼滤波原理：

卡尔曼滤波器是在估计线性系统状态的过程中，以最小均方误差为目的而推导出的几个递推数学等式,也可以从贝叶斯推断的角度来推导。这种滤波器是将过去的测量估计误差合并到新的测量误差中来估计将来的误差。

通俗的来说就是：首先需要建立系统状态方程和观测方程，系统状态方程是要估计的量的状态转移方程，即怎么根据当前时刻的状态量估计下一时刻的状态变量，观测方程是状态变量与观测值得关系方程，观测值是我们从实际系统中得到的一个观测值，用以对系统变量的量测的结果。

卡尔曼滤波分两步：预测（估计）与更新

预测又分两步，一个是根据上一时刻的状态变量递推当前时刻的系统变量，即X(k-1,k)=F*X(k-1,k-1)，F为状态转移矩阵，X(k-1,k)也被称为系统状态先验估计，这一步完全是你根据自己建立的状态方程进行的预测，所以还需要用实际的数据进行修正；另一个是对当前的系统误差协方差的先验估计。

更新则是对当前的系统状态以及当前的系统误差协方差进行更新。

卡尔曼滤波核心：5个公式

系统状态先验估计，得到预测值

系统状态误差（预测值与真实值）协方差先验估计

计算最优卡尔曼增益

更新系统状态，得到估计值

更新系统状态误差（估计值与真实值）协方差

首先，我们先要引入一个离散控制过程的系统。该系统可用一个线性随机微分方程（Linear Stochastic Difference equation）来描述：

　　X(k)=A X(k-1)+B U(k)+W(k)

　　再加上系统的测量值：

　　Z(k)=H X(k)+V(k)

　　上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，他们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。

　　对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。下面我们来用他们结合他们的covariances 来估算系统的最优化输出（类似上一节那个温度的例子）。

　　首先我们要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：

　　X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

　　式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

　　到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance还没更新。我们用P表示covariance：

　　P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

　　式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

　　现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：

　　X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

　　其中Kg为卡尔曼增益(Kalman Gain)：

　　Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

　　到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：

　　P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)

　　其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

　　卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5 个基本公式。根据这5个公式，可以很容易的实现计算机的程序。

卡尔曼滤波应用：

卡尔曼滤波的应用范围非常广泛，为了让读者进一步理解卡尔曼滤波，下面讲解几个实际应用中的例子。

1.温度采集系统

温度采集系统的硬件电路构成：温度传感器+单片机（具有ADC功能）+电源。

误差分析，误差主要有两点

系统误差：温度传感器自身的误差；

量测误差：单片机ADC采集时候的误差；

下面是用matlab模拟仿真的程序

clc; clear

N=200;

for t=1:N

z(t)=25+3*randn; %测量值

end

R=std(z)^2 %测量噪声

V=0.1*randn(1,N);

Q=std(V)^2 %模型噪声

p(1)=10;

x(1)=0;

for t=2:N;

x1(t)=x(t-1); %预测状态

p1(t)=p(t-1)+Q; %预测估计协方差

k(t)=p1(t)/(p1(t)+R); %最优卡尔曼增益

x(t)=x1(t)+k(t)*(z(t)-x1(t)); %更新状态估计

p(t)=(1-k(t))*p1(t); %更新协方差估计

end

t=1:N;

plot([0,N],[25,25],'g',t,z,'b',t,x,'r');

legend('真实值','测量值','估计值');

温度测量系统仿真图

2.MPU6050（6轴运动传感器）数据处理C语言代码

/**************************极光电子科技**********************

* 文件名：kalman_filter.c

* 描述 : 卡尔曼滤波

* 实验平台：stm32f103

* 版本：1.0

**********************************************************************************/

#include "kalman_filter.h"

#include <math.h>

extern float g_f_jiaosudu; //系统倾斜角速度

extern float g_f_jiaodu; //系统倾斜角度

float Angle=0,Gyro_x=0; //系统滤波后倾斜角度/角速度

//*********卡尔曼参数************

float Q_angle=0.0001; //角度估计噪声 (0.0001)

float Q_gyro= 0.0001;//角速度估计噪声 (0.0001)

float R_angle=0.5; //角度量测噪声值 (0.5)

float dt=0.002; //dt为kalman滤波器采样时间;

char C_0 = 1;

float Q_bias=0, Angle_err=0;

float PCt_0=0, PCt_1=0, E=0;

float K_0=0, K_1=0, t_0=0, t_1=0;

float Pdot[4] ={0,0,0,0};

float PP[2][2] = { { 1, 0 },{ 0, 1 } };

//*********************************************************

// 卡尔曼滤波

//*********************************************************

//Kalman滤波，72MHz的处理时间约0.2ms；

void Kalman_Filter(float jiaosudu_1_,float jiaodu_1_)

{

Angle+=(jiaosudu_1_ - 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;

Angle_err = jiaodu_1_ - Angle;

PCt_0 = C_0 * PP[0][0];

PCt_1 = C_0 * PP[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;

K_1 = PCt_1 / E;

t_0 = PCt_0;

t_1 = C_0 * PP[0][1];

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;

Angle+= K_0 * Angle_err; //后验估计，得到当前时刻的最优估计角度值

Q_bias+= K_1 * Angle_err; //后验估计

Gyro_x = jiaosudu_1_ - Q_bias;//后验估计,得到当前时刻的最优估计角速度

/*-------获取滤波后的角度值和角速度值--------*/

g_f_jiaosudu=Gyro_x;//角速度值

g_f_jiaodu=Angle; //角度值

}

备注

经典的卡尔曼滤波只适用于线性且满足高斯分布的系统，但实际工程中并不是这么简单，比如飞行器在水平运动时有可能伴随着自身的自旋，此时的系统并不是线性的，这时就需要应用扩展卡尔曼滤波（EKF）来解决这种情况。

今天就不多说了，以后小编在详细介绍扩展卡尔曼滤波。

关注小编，更多精彩内容等着你。

解读——卡尔曼滤波（一）

关注不迷路奥^~^

卡尔曼滤波 是信息处理领域最为著名的数据处理算法之一了，被广泛的运用到航天、汽车、工程等领域（哈哈，先来吹嘘一番）。在生活中的手机、电脑游戏等领域也被广泛运用。可以说卡尔曼滤波已经运用到我们生活中的角角落落。

在做单片机控制领域中，卡尔曼滤波算法应该是必不可少的了，为什么来？单片机控制的一般流程是：单片机采集传感器的数据，然后处理数据，在输出数据。等等~，你怎么知道传感器传来的数据都是正确的呢？有的小伙伴可能会说“我们平常直接使用传感器传感器传来的数据进行操作，没啥问题啊”，这么说的娃子，只能说你搞控制搞的还不深啊（苦海无边，回头是岸，你还有机会ヾ(ﾟ∀ﾟゞ)，你刚出港口、、、别人可能已经进入大海中心了，不是不想回头，找不到方向了啊ヽ(*。>Д<)o゜）说句玩笑话。

人们平常使用的传感器在采集数据时，总会受到各种干扰（天气、噪声、供电不稳等），这使得传来的数据充满不确定性，卡尔曼滤波就是在这些不确定性的数据中筛选出有效的数据 。下面来看一下卡尔曼滤波的面貌吧。

卡尔曼滤波方程

卡尔曼滤波主要包括两个阶段：预测阶段 和测量更新阶段 。预测阶段 是指用公式根据上一时间段的结果推论出当前的结果 （即根据t-1时刻的数据推论出t时刻的数据）（注意：预测的数据具有不确定性）。测量更新阶段 即用传感器测量一下系统当前的数据 （注意：此处的测量数据具有噪声干扰 ）。最后在t时刻的实际数据就是预测数据和测量数据的叠加（注意：这里的叠加不是简单地相加、减，或者相乘、除 ）。

预测方程 如下：