卡尔曼滤波器

只看该作者 · 2016-7-15 15:55

卡尔曼滤波器原理

一、什么是卡尔曼。

跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！1960年卡尔曼在他的博士论文和发表的论文《A New Approach toLinear Filtering and Prediction Problems》（线性滤波与预测问题的新方法中提出了这种算法。

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

二、卡尔曼滤波器的通俗理解。

这是网上的关于解释卡尔曼滤波器原理的一个经典例子：

假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（WhiteGaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（GaussianDistribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。

好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

假如我们要估算k时刻的是实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。

由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg^2=5^2/(5^2+4^2)，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.56度）的偏差。算法如下：((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
就是这样，卡尔曼滤波器就不断的把covariance递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的covariance。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！

所以通过这个例子我们大概能够理解到：我们是通过上一状态的最优估算值以及这次的测量值，以及卡尔曼增益（什么事卡尔曼增益，到这里，我只能先说就是例子里面的Kg喽）可以得到现在状态的最优估算值。

只看该作者 · 2016-7-15 16:01

三、卡尔曼滤波器算法的原理。

说白了，卡尔曼滤波器就是五个公式达到滤波的效果的：

（1）预测当前系统状态的公式

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

（2）对应当前系统状态值的协方差（covariance）

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

（3）计算卡尔曼增益Kg的公式

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

（4）得到当前状态的最优估算值

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

（5）对应当前状态最优估算值的协方差

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

对了，除了这五个公式还需引入一个有关系统的线性随机微分方程：

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

以及系统的测量值方程：

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

这就是神奇的卡尔曼滤波的核心东西了，理解了这五个公式也就理解了卡尔曼滤波器的原理了。

四、一个卡尔曼滤波器的简单例子。

这里我们结合第二第三节，举一个非常简单的例子来说明卡尔曼滤波器的工作过程。所举的例子是进一步描述第二节的例子，而且还会配以程序模拟结果。

根据第二节的描述，把房间看成一个系统，然后对这个系统建模。当然，我们见的模型不需要非常地精确。我们所知道的这个房间的温度是跟前一时刻的温度相同的，所以A=1。没有控制量，所以U(k)=0。因此得出：
X(k|k-1)=X(k-1|k-1) ……….. (6)
式子（2）可以改成：
P(k|k-1)=P(k-1|k-1) +Q ……… (7)

因为测量的值是温度计的，跟温度直接对应，所以H=1。式子3，4，5可以改成以下：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ……… (9)
P(k|k)=（1-Kg(k)）P(k|k-1) ……… (10)

现在我们模拟一组测量值作为输入。假设房间的真实温度为25度，我模拟了200个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的高斯白噪声（在图中为蓝线）。

为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐的收敛。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。我选了X(0|0)=1度，P(0|0)=10。

该系统的真实温度为25度，图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果（该结果在算法中设置了Q=1e-6，R=1e-1）。

所以到此，卡尔曼滤波器就介绍的差不多了，相信你应该已经理解卡尔曼滤波器喽。

（什么，你还不明白? 那好吧，下面是一个流传在网上的卡尔曼滤波器的C源码，你拿去吧。）

//卡尔曼滤波---------------------------------------------------------

float Q_angle=0.001;//0.001

float Q_gyro=0.003;//0.003

float R_angle=0.5;//0.5

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

char C_0 = 1;

float Q_bias, Angle_err;

float PCt_0, PCt_1, E;

float K_0, K_1, t_0, t_1;

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

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

//卡尔曼函数------------------------------------------------------

float Kalman_Filter(float Accel,float Gyro)//输入angleAx 和 gyroGy

{

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;

Angle_err = Accel - Angle; //zk-先验估计

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_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度

return Angle;

}

只看该作者 · 2016-7-17 14:33

这个卡尔曼滤波看着很好用啊

1万 · 2016-7-17 22:26

卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。
斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。
数据滤波是去除噪声还原真实数据的一种数据处理技术, Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用.

1万 · 2016-7-17 23:54

((1-Kg)*5^2)^0.5=2.35。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的2.35就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
原来如此

只看该作者 · 2016-7-18 22:55

数字滤波器
数字滤波器由数字乘法器、加法器和延时单元组成的一种算法或装置。数字滤波器的功能是对输入离散信号的数字代码进行运算处理，以达到改变信号频谱的目的。

如果采用通用的计算机，随时编写程序就能进行信号处理的工作，但处理的速度较慢。如果采用专用的计算机芯片，它是按运算方法制成的集成电路，连接信号就能进行处理工作，处理的速度飞快，但功能不易更改。如果采用可编程的计算机芯片，那么，装入什么程序机器就能具有什么功能。这种可编程芯片的优点很多，是现代电子产品的首选。如果是对模拟信号进行处理，则需要添加模数转换器和数模转换器。

只看该作者 · 2016-7-19 20:58

数字滤波算法

随机误差是有随机干搅引起的，其特点是在相同条件下测量同一个量时，其大小和符号做无规则变化而无法预测，但多次测量结果符合统计规律。为克服随机干搅引入的误差，硬件上可采用滤波技术，软件上可以采用软件算法实现数字滤波，其算法往往是系统测控算法的一个重要组成部分，实时性很强，采用汇编语言来编写。

采用数字滤波算法克服随机干搅引入的误差具有以下几个优点：

（1）数字滤波无须硬件，只用一个计算过程，可靠性高，不存在阻抗匹配问题，尤其是数字滤波可以对

频率很高或很低的信号进行滤波，这是模拟滤波器做不到的。

（2）数字滤波是用软件算法实现的，多输入通道可用一个软件“滤波器”从而降低系统开支。

（3）只要适当改变软件滤波器的滤波程序或运行参数，就能方便地改变其滤波特性，这个对于低频、脉冲

干搅、随机噪声等特别有效。

常用的数字滤波器算法有程序判断法、中值判断法、算术平均值法、加权滤波法、滑动滤波法、低通滤波法和复合滤波法。

1．程序判断法：

程序判断法又称限副滤波法，其方法是把两次相邻的采样值相减，求出其增量（以绝对值表示）。然后与两次采样允许的最大差值△Y进行比较，△Y的大小由被测对象的具体情况而定，若小于或等于△Y，则取本次采样的值；若大于△Y，则取上次采样值作为本次采样值，即

yn - yn-1|≤△Y，则yn有效，

yn -yn-1|＞△Y，则yn-1有效。

式中 yn ——第n次采样的值；

Yn-1——第（n-1）次采样的值；

△Y——相邻两次采样值允许的最大偏差。

设R1和R2为内部RAM单元，分别存放yn-1和yn，滤波值也存放在R2单元，采用MCS-51单片机指令编写的程序判断法子程序如下：付表

2．中值滤波法即对某一参数连续采样N次（一般N为奇数），然后把N次采样值按从小到大排队，再取中间值作为本次采样值。

设DATA为存放采样值的内存单元首地址，SAMP为存放滤波值的内存单元地址，N为采样值个数，用MCS-51指令编写的中值滤波子程序如下：副表

3．算术平均值滤波算法

算术平均滤波法就是连续取N次采样值进行算术平均，其数学表达式是：

Y=∑yi

~y=1/N ∑ yi

i=1……N

式中 ~y——N个采样值的算术平均值；

Yi ——第i个采样值；

设8次采样值依次存放在地址DATA开始的连续单元中，滤波结果保留在累加器A中，程序如下：副表

4.加权平均滤波法

算术平均滤波法存在前面所说的平滑和灵敏度之间的矛盾。采样次数太少，平滑效果差，次数太多，灵敏度下降，对参数的变化趋势不敏感。协调两者关系，可采用加权平均滤波，对连续N次采样值，分别乘上不同的加权系数之后再求累加和，加权系数一般先小后大，以突出后面若干采样的效果，加强系统对参数的变化趋势的辩识，各个加权系数均为小于1的小数，且满足总和等于1的约束条件，这样，加权运算之后的累加和即为有效采样值。为方便计算，可取各个加权系数均为整数，且总和为256，加权运算后的累加和除以256（即舍去低字节）后便是有效采样值。

设每批采样8个数据，依次存放在地址DATA开始的单元中，各加权系数是用一个表格存放在ROM中，MCS-51指令编写的算术平均滤波程序如下：副表

5.滑动平均滤波法：

以上介绍的各种平均滤波算法有一个共同点，即每取得一个有效采样值必须连续进行若干次采样，当采样速度较慢（如双积分型A/D转换）或目标参数变化较快时，系统的实时性不能保证，滑动平均滤波算法只采样一次，将这一次采样值和过去的若干次采样值一起求平均，得到的有效采样值即可投入使用，如果取N个采样值求平均，RAM中必须开辟N个数据的暂存区。每新采样一个数据便存入暂存区，同时去掉一个最老的数据，保持这N个数据始终是最近的数据，这种数据存放方式可以用环行队列结构方便的实现。设环行队列为40H-4FH连续16个单元，RO作为队尾指针，滤波程序如下：副表

6.低通滤波法：

将普通硬件RC低通滤波器的微分方程用差分方程来表求，变可以采用软件算法来模拟硬件滤波的功能，经推导，低通滤波算法如下：

Yn=a* Xn+ （1-a） *Yn-1

式中 Xn——本次采样值

Yn-1——上次的滤波输出值；

a——滤波系数，其值通常远小于1；

Yn——本次滤波的输出值。

由上式可以看出，本次滤波的输出值主要取决于上次滤波的输出值（注意不是上次的采样值，这和加权平均滤波是有本质区别的），本次采样值对滤波输出的贡献是比较小的，但多少有些修正作用，这种算法便模拟了具体有教大惯性的低通滤波器功能。滤波算法的截止频率可用以下式计算：

fL= a/2Pit pi为圆周率 3.14…

式中 a——滤波系数；

t——采样间隔时间；

例如：当t=0.5s（即每秒2次），a=1/32时；

fL=（1/32）/（2*3.14*0.5）=0.01Hz

当目标参数为变化很慢的物理量时，这是很有效的。另外一方面，它不能滤除高于1/2采样频率的干搅信号，本例中采样频率为2Hz，故对1Hz以上的干搅信号应采用其他方式滤除，

低通滤波算法程序于加权平均滤波相似，但加权系数只有两个：a和1-a。为计算方便，a取一整数，1-a用256-a，来代替，计算结果舍去最低字节即可，因为只有两项，a和1-a，均以立即数的形式编入程序中，不另外设表格。虽然采样值为单元字节（8位A/D）。为保证运算精度，滤波输出值用双字节表示，其中一个字节整数，一字节小数，否则有可能因为每次舍去尾数而使输出不会变化。

设Yn-1存放在30H（整数）和31H（小数）两单元中，Yn存放在32H（整数）和33H（小数）中。滤波程序如下：副表

结束语：

微型计算机在仪器仪表系统中的成功应用，使传统的电子仪器以及复杂的跟踪测量装置发生了许多革命性变化。其中一个突出表现就是一个系统中包含了智能性运作。微机具有很强的分析和运算能力，智能系统可完成复杂的数据处理，智能系统采用软件硬件想结合的方法进行随机误差的数字滤波和系统误差的修正，可以实现实时修正，较准测量数据，这些都是传统仪器难以比拟的。

只看该作者 · 2016-7-19 20:59

复制

#include

#include

#include

#include <./Atmel/at89x52.h>

#include "source.h"

main()

{

filter_1();

filter_2();

filter_3();

filter_4();

filter_5();

filter_6();

filter_7();

filter_8();

filter_9();

filter_10();

}

unsigned char get_ad(void){

static unsigned char i;

return i++;

}

void delay(void){

unsigned char i=0;

while(1){

i++;

if(i>20) return;

}

}

 

#define A 10 //设置两次采样允许的最大偏差值

char value;  //上次采用后的有效值变量

char filter_1(void){

char  new_value;  //本次采样值变量

new_value=get_ad();  //读入本次采样值

if((new_value-value>A)||(value-new_value>A)) //比较是否超出最大偏差值

return  value; //如果超出，返回上次的有效值作为本次的有效值

return  new_value;// 如果没有超出，返回本次的采样值作为本次的有效值

}

 

#define N 11 //设置连续采样的次数

 

char filter_2(void){

char  value_buf[N]; //缓存N次采样值的存储变量

char  count,i,j,temp; //i，j是冒泡排序的下标变量，count是采样数据读入的下标变量

//temp是临时变量

for(count=0;count

{

value_buf[count]=get_ad();

delay();

}

for(j=0;j

{

for(i=0;i

{

if(value_buf[i]>value_buf[i+1])

{

temp=value_buf[i];

value_buf[i]=value_buf[i+1];

value_buf[i+1]=temp;

}

}

}

return value_buf[(N-1)/2]; //将排序后N个采样值的中间值作为最后结果返回

}

 

 

#undef N

#define N 12 //设置每组参与平均运算的采样值个数

 

char filter_3(){

int  sum=0; //求和变量，用于存储采样值的累加值

char count;//采样数据读入的下标变量

for(count=0;count

{

sum+=get_ad();

delay();

}

return (char)(sum/N); //讲累加值进行平均计算作为返回值

}

 

#undef N

#define N 12 //设置FIFO队列的长度

char  value_buf[N];//FIFO队列变量

char  i=0; //队列的下标变量

 

char filter_4(){

char count;

int  sum=0;

value_buf[i++]=get_ad();

if(i==N)  i=0;

for(count=0;count

sum+=value_buf[count];

return(char)(sum/N);

}

 

 

#undef N

#define N 12 //设置每组采样值的数量

char filter_5()

{

char count,i,j,temp; //i，j是冒泡排序的下标变量，count是采样数据读入的下标变量

char value_buf[N]; // 缓冲N个采样值的存储变量

int  sum=0; //求和变量，用于存储采样值的累加值

for  (count=0;count

{

value_buf[count] = get_ad();

delay();

}

for (j=0;j

{

for (i=0;i

{

if ( value_buf[i]>value_buf[i+1] )

{

temp = value_buf[i];

value_buf[i] = value_buf[i+1];

value_buf[i+1] = temp;

}

}

}

for(count=1;count

sum += value_buf[count]; //去掉两端的最小和最大采样值，对中间的N-2个采样值求和

return (char)(sum/(N-2));// 返回中间N-2个采样值的平均值

}

 

 

 

#undef A

#undef N

#define A 10 //设置两次采样允许的最大偏差值

#define N 12 //设置每组参与平均运算的采样值个数

char value;  //上次采用后的有效值变量

 

char filter_6()

{

char  new_value;  //本次采样值变量

int  sum=0; //求和变量，用于存储采样值的累加值

char count;//采样数据读入的下标变量

for(count=0;count

{

new_value=get_ad();  //读入本次采样值

if((new_value-value>A)||(value-new_value>A)) //比较是否超出最大偏差值

new_value=value; //如果超出，返回上次的有效值作为本次的有效值

sum+=new_value; //累加采样的有效值

value=new_value;

delay();

}

return (char)(sum/N); //将累加值进行平均计算作为返回值

}

 

 

#define COE 50 //定义加权系数

char value; //上一个采样值变量

char filter_7()

{

char  new_value; //本次采样值变量

new_value = get_ad();

return (100-COE)*value + COE*new_value; //返回的本次滤波结果

}

 

 

 

 

#undef N

#define N 12  //设置FIFO队列的长度

 

char code coe[N] = {1,2,3,4,5,6,7,8,9,10,11,12}; //加权系数

char code sum_coe = 1+2+3+4+5+6+7+8+9+10+11+12;

char filter_8()

{

char count; //采样数据读入的下标变量

char value_buf[N]; //缓存N个采样值的存储变量

int  sum=0; //求和变量，用于存储采样值的累加值

for (count=0;count

{

value_buf[count] = get_ad(); //读入采样值

delay();

}

for (count=0;count

sum += value_buf[count]*coe[count]; //累加采样值和系数的乘积

return (char)(sum/sum_coe); //累加值与系数和相除作为返回结果

}

 

 

#undef N

#define N 12 //设置计数器溢出值

char filter_9()

{

char count=0; //计数变量

char new_value; //本次采样值变量

new_value = get_ad(); //读入本次采样值

while (value !=new_value);

{

count++; //计数器加1

if (count>=N)   return new_value; //如果本次采样值与当前有效值不相等，

//且计数器溢出，返回本次采样值

delay();

new_value = get_ad();

}

return value; //如果本次采样值与当前有效值相等，则返回当前有效值

}

 

 

 

 

 

 

#undef A

#undef N

#define A 10 //设置两次采样允许的最大偏差值

#define N 12 //设置计数器溢出值

char value;  //有效值变量

 

char filter_10()

{

char count=0; //计数变量

char new_value; //本次采样值变量

new_value = get_ad(); //读入本次采样值

if((new_value-value>A)||(value-new_value>A)) //比较是否超出最大偏差值

new_value=value; //如果超出，返回有效值作为本次的采样有效值

while (value !=new_value);

{

count++; //计数器加1

if (count>=N)   return new_value; //如果本次采样值与当前有效值不相等，

//且计数器溢出，返回本次采样值

delay();

new_value = get_ad();

}

return value; //如果本次采样值与当前有效值相等，则返回当前有效值

}

1万 · 2016-7-19 21:36

卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等

1万 · 2016-7-19 23:42

对应当前系统状态值的协方差（covariance）P(k|k-1)=A P(k-1|k-1) A’+Q

只看该作者 · 2016-7-23 22:10

卡尔曼滤波原理

过程方程：

X(k+1) = A X(k) + B U(k) + W(k) >>>>式1

量测方程：

Z(k+1) = H X（k+1）+ V(k+1) >>>>式2

A和B是系统参数，对于多模型系统，他们为矩阵；H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声，他们的协方差分别是Q，R。为了不失一般性，下面的讨论中将X,Z都视为矩阵，其中X是m行的单列矩阵，Z是n行的单列矩阵。

只看该作者 · 2016-7-23 22:11

说明：下面的表达式中，不带前缀的量都代表实际量，其小括号里面的“k”或“k+1”代表该量是第k或第k+1时刻的实际量，如“Z(k+1)”就代表第k+1时刻的实际测量值；

带前缀“^”的量都代表预测量，如果小括号里面是“k+1|k”，就代表k+1时刻的先验预测值，如果小括号里面是“k+1|k+1”，就代表k+1时刻的后验预测值；（测量值可以通过测量得到，所以只有先验预测，没有后验预测。而实际状态值无法得知，既有先验预测，又有后验预测）

带前缀“~”的量都代表与预测值对应的偏差值。

实际状态值与先验预测状态值的偏差 = 实际状态值 – 先验预测状态值

~X(k+1|k) = X(k+1) - ^X(k+1|k) >>>> 式3

实际测量值与先验预测测量值的偏差 = 当前测量值 - 先验预测测量值

~Z(k+1|k) = Z(k+1) - ^Z(k+1|k) >>>>式4

并且

先验预测测量值 = 转换矩阵H * 先验预测状态值

^Z(k+1|k) = H ^X(k+1|k) >>>> 式5

得到测量值后，再对当前状态值X(k+1) 进行后验预测（设后验预测值为 ^Z(k+1|k+1) ），则后验预测值（同时也是最终预测值）的偏差为

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1) >>>>式6

为了得到当前状态值X(k+1)，根据式3，需要：

X(k+1) = ^X(k+1|k) + ~X(k+1|k) >>>> 式7

上式中，我们可以通过卡尔曼公式1（见附注2）计算出^X(k+1|k)，但我们无法得知实际状态值X(k+1)，因而~X(k+1|k) 也无法得知。我们最终的目的是得出一个比较接近实际状态值X(k+1)的滤波值^X(k+1|k+1)，根据式7，只要能准确的估计出~X(k+1|k)即可。

~X(k+1|k)本身虽无法得知，但~Z(k+1|k) 却可以通过测量得到，而且它们二者存在一定的相关性。不妨再设存在一个矩阵K（m行n列矩阵），能使得

~X(k+1|k) = K * ~Z(k+1|k) >>>>式8

那么最终的预测任务其实就是找到K。由于~X(k+1|k)和~Z(k+1|k)都是单列矩阵，因此不难看出，满足式8的矩阵K应有无穷多个。矩阵K中第i行第j列反映了量测变量偏差矩阵~Z(k+1|k)的第j个元素对状态变量偏差矩阵~X(k+1|k)的第i个元素的贡献。因此矩阵K的物理意义很明显，K的第i行第j列的元素表示：对于第i个待测的状态量来说，第j个测量仪器测到的偏差的可信度。某个测量值对应的可信度越高，滤波器越“相信”该测量值。

既然满足条件的K有无穷多个，那应该使用哪个K呢？实际上，我们并不知道~X(k+1|k)的值，所以也就无法直接计算出K，而只能通过某种方法找到一个Kg，使得将Kg带入式8后，等号两边的差（的平方）的期望尽可能小。

我们最终的预测值或滤波值是后验预测值^X(k+1|k+1)，因此最后的预测也应使 ~X(k+1|k+1) 的期望为0且方差最小（这与让8式两端的差最小是一致的，下面的式9体现了这一点），这样预测值才最可靠。

只看该作者 · 2016-7-23 22:11

下面详细说明。

^X(k+1|k+1) = ^X(k+1|k) + Kg * ~Z(k+1|k) （后验预测的状态值）

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1) （后验预测的偏差）

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1)

= ( ^X(k+1|k) + ~X(k+1|k) ) - ( ^X(k+1|k) + Kg * ~Z(k+1|k) )

= ~X(k+1|k) - Kg * ~Z(k+1|k)

>>>>式9

~Z(k+1|k) = Z(k+1) - ^Z(k+1|k)

= ( H X（k+1）+ V(k+1) ) - ( H ^X(k+1|k) )

= H ( X（k+1）-^X(k+1|k) ) + V(k+1)

= H ~ X(k+1|k) + V(k+1) >>>>式10

接下来的分析中，为了更直观的说明卡尔曼滤波的原理，我们用几何方法来解释。这时，~X和~Z矩阵中的每个元素应看做向量空间中的一个向量而不再是一个单纯的数。这个向量空间（统计测试空间）可以看成无穷多维的，每一个维对应一个可能的状态。~X和~Z矩阵中的每个元素向量都是由所有可能的状态按照各自出现的概率组合而成（在测量之前，~X和~Z 的实际值都是不可知的）。~X和~Z中的每个元素向量都应是0均值的，他们与自己的内积就是他们的协方差矩阵。我们无法给出~X和~Z中每个元素向量的具体表达，但我们通过协方差矩阵就可以知道所有元素向量的模长，以及相互之间的夹角（从内积计算）。

为了方便用几何方法解释，我们假设状态变量X是一个1行1列的矩阵（即只有一个待测状态量），而量测变量Z是一个2行1列的矩阵（即有两个测量仪器，共同测量同一个状态量X），也就是说，m=1，n=2。矩阵X中只有X[1]一项，矩阵Z中有Z[1]和Z[2]两项。Kg此时应是一个1行2列的矩阵，两个元素分别记作Kg1 和 Kg2 。H和V此时应是一个2行1列的矩阵。

将矩阵表达式9和10按元素展开：

~X(k+1|k+1)[1] = ~X(k+1|k)[1] - (Kg1 * ~Z(k+1|k)[1] + Kg2 * ~Z(k+1|k)[2] ) >>>> 式9i

~Z(k+1|k) = H ~ X(k+1|k) + V(k+1) >>>>式10i

~X(k+1|k) 中各个元素（向量）的线性组合可以产生一个m维或更低维的向量子空间Vx，这里，按照我们的假设，m=1，所以Vx应是一维的；同时V(k+1)中的各个元素（向量）的线性组合也可以产生一个n维或更低维的向量子空间Vv，这里，按照我们的假设，n=2，所以Vv应是二维的。由于V(k+1)中的每一项与~X(k+1|k)中的每一项都不相关（见附注1），故这两个子空间相互垂直。如下图所示。式10i所体现的~Z(k+1|k)、H~ X(k+1|k)、V(k+1) 三者之间的几何关系，也在下图中描绘了出来。

只看该作者 · 2016-7-23 22:13

从上图中可以看出，~Z(k+1|k)中各个元素（向量）的线性组合也可以产生一个n维或更低维的向量子空间Vz，这里已假设n=2，所以Vz是一个二维的平面，就是上图中两条红色的线所构成的平面。

通过先验预测的协方差矩阵（见卡尔曼公式2），可以得到~X(k+1|k)中各个元素的模长以及彼此间的夹角。这是因为协方差矩阵中的第i行第j列其实就代表了~X(k+1|k)中第i个元素向量与第j个元素向量的内积。

通过测量可以得到新息协方差（见卡尔曼公式3的分母部分），进而可以知道~Z(k+1|k)中各个元素的模长以及彼此间的夹角。

通过已知的量测噪声协方差矩阵R，可以得出V(k+1) 中各个元素的模长以及彼此间的夹角。

最后根据~X(k+1|k+1)[1]与Y垂直以及图1中所示的几何关系，用高中学的立体几何和向量知识就可以求得两个Kg的值了。如果将向量的内积都用协方差矩阵表示，就会发现，我们最后求得的Kg，其实就是卡尔曼公式3。

（上面讨论的是较低次的卡尔曼滤波，只有一个待测量，两个测量仪器。这种情况还是比较常见的，比如倾角测量系统中，我们用加速度计和陀螺仪共同测量倾角。对于更高次的卡尔曼滤波，X和Z都是多行矩阵时，用几何方法已经无法直观解释，只能用矩阵分析的方法证明。求解Kg的详细过程参考《卡尔曼滤波器及其应用基础》国防工业出版社敬喜编）

卡尔曼滤波的核心过程，就是求解能使得

E { ~X(k+1|k+1) ’ * ~X(k+1|k+1) }

取最小值的Kg增益矩阵的过程，~X(k+1|k+1)’代表的是~X(k+1|k+1)的转置（这里~X(k+1|k+1)中的元素代表数值，不是向量）。前面已经提到过，卡尔曼增益矩阵Kg中的元素，都代表测量仪器测到的偏差的可信度，或者叫估计权重。

只看该作者 · 2016-7-23 22:14

附注1：

(a). v(k+1)中的每一项与~X(k+1|k)中的每一项都不相关

~X(k+1|k) = X(k+1) - ^X(k+1|k)

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

= ( A X(k) + B U(k) + W(k) ) - ( A X(k) + B U(k) - A ~X(k|k))

= W(k) + A ~X(k|k)

= W(k) + A ( ~X(k|k-1) - Kg(k)* ~ Z(k|k-1) ) -----这一步利用了式9

= W(k) + A ( ~X(k|k-1) - Kg(k)* ( H ~X(k|k-1) + v(k) ) ) ------这一步利用了式10

= W(k) - A Kg(k) *v(k) + A ( I - Kg(k) * H ) ~X(k|k-1)

上式最后一行出现了~X(k|k-1)，可见~X(k+1|k)可以递归表示。而且递归式中的过程噪声W(k)与v(k+1)不相关，同时由于 v本身是白噪声，所以 v(k+1)与v(k)亦不相关（白噪声的自相关是δ函数），因此通过递推式可以判断v(k+1)与~X(k+1|k)不相关。

(b). w(k+1)中的每一项与~X(k+1|k+1)中的每一项都不相关，w(k+1)中的每一项与~X(k+1|k)中的每一项都不相关。

~X(k+1|k+1) = X(k+1) - ^X(k+1|k+1)

= ( ^X(k+1|k) + ~X(k+1|k) ) - ( ^X(k+1|k) + Kg(k+1)*~Z(k+1|k) )

= ~X(k+1|k) - Kg(k+1)*~Z(k+1|k)

= ~X(k+1|k) - Kg(k+1)* ( H ~X(k+1|k) + v(k+1) )

= - Kg(k+1)* v(k+1) + ( I - Kg(k+1) * H ) ~X(k+1|k)

我们已经知道w(k+1)与v(k+1)不相关，因此只要~X(k+1|k+1)与上式的第二项也不相关，就说明结论(b)成立。根据(a)中的结论，~X(k+1|k)的递归展开式中出现的 v(k) ，w(k) ， v(k-1)，w(k-1)等等，显然 w(k+1)与 v (m=k，k-1……) 都不相关，另外，由于w(k+1)的自相关为δ函数，因此w(k+1)与 w(m=k，k-1……) 也不相关，也就得出w(k+1)与~X(k+1|k) 不相关。

进而可知，w(k+1)与~X(k+1|k+1)不相关。

正是因为 (a) (b)中的两个不相关，卡尔曼公式中的预测协方差矩（卡尔曼公式(2)）和新息协方差矩阵（卡尔曼公式(3)中的“分母”部分）才可以是简单的加式。

附注2：卡尔曼滤波的五个公式

先验预测值与先验预测协方差矩阵的计算。求解协方差时，都认为预测值的期望是实际值。因此，^X(k+1|k)的协方差矩阵同样也是 ~X(k+1|k) 的协方差矩阵，又因为偏差~X(k+1|k)的期望是0，因此协方差矩阵反映了~X(k+1|k)在向量空间中的模长。注意，协方差矩阵都是对称矩阵。

X(k+1|k)=A X(k|k)+B U(k) (1)

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

卡尔曼增益矩阵的计算。量测预测值为

Z(k+1|k) = H X(k+1|k)

新息协方差见公式（3）中的“分母”部分。量测预测值的期望是实际量测值。因此，^Z(k+1|k)的协方差矩阵同样也是 ~Z(k+1|k) 的协方差矩阵，又因为偏差~Z(k+1|k)的期望是0，因此协方差矩阵反映了~Z(k+1|k)在向量空间中的模长。

Kg(k+1)= P(k+1|k) H’/ (H P(k+1|k) H’ + R(k+1)) (3)

后验预测值与后验预测协方差矩阵的计算

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

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

只看该作者 · 2016-7-23 22:19

最优化自回归数据处理算法

只看该作者 · 2016-7-23 23:06

广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

1万 · 2016-7-24 16:33

关键内涵是：通过上一状态的最优估算值以及这次的测量值，以及卡尔曼增益（什么事卡尔曼增益，到这里，我只能先说就是例子里面的Kg喽）可以得到现在状态的最优估算值。

只看该作者 · 2016-7-24 16:49

根据功能，那么前面的数据量越精确越好了。

只看该作者 · 2016-7-24 18:22

卡尔曼滤波是很重要的，如果想成为高手，必须熟练的掌握原理和使用方法。

卡尔曼滤波器

技术新星奖章

湍急之河流

社区建设奖章

经常做客