卡尔曼滤波算法与matlab实现.docx

资源描述

卡尔曼滤波算法与matlab实现.docx

《卡尔曼滤波算法与matlab实现.docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波算法与matlab实现.docx（22页珍藏版）》请在冰豆网上搜索。

卡尔曼滤波算法与matlab实现.docx

卡尔曼滤波算法与matlab实现

covariance，A’表示A的转置矩阵，Q是系统过程的covariance。

式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

现在我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。

结合预测值和测量值，我们可以得到现在状态（k）的最优化估算值X（k|k）：

X（k|k）=X（k|k-1）+Kg（k）（Z（k）-HX（k|k-1））………（3）

其中Kg为卡尔曼增益（KalmanGain）：

Kg（k）=P（k|k-1）H’/（HP（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个公式，可以很容易的实现计算机的程序。

下面，用Matlab程序举一个实际运行的例子。

4．简单例子

（ASimpleExample）

这里我们结合第二第三节，举一个非常简单的例子来说明卡尔曼滤波器的工作过程。

所举的例子是进一步描述第二节的例子，而且还会配以程序模拟结果。

根据第二节的描述，把房间看成一个系统，然后对这个系统建模。

当然，我们见的模型不需要非常地精确。

我们所知道的这个房间的温度是跟前一时刻的温度相同的，所以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）。

clear

N=200;

（1）=0;

w=randn（1,N）

（1）=0;

a=1;

fork=2:

x（k）=a*x（k-1）+w（k-1）;

end

V=randn（1,N）;

q1=std（V）;

Rvv=q1.^2;

q2=std（x）;

Rxx=q2.^2;

q3=std（w）;

Rww=q3.^2;

c=0.2;

Y=c*x+V;

（1）=0;

fort=2:

p1（t）=a.^2*p（t-1）+Rww;

b（t）=c*p1（t）/（c.^2*p1（t）+Rvv）;

s（t）=a*s（t-1）+b（t）*（Y（t）-a*c*s（t-1））;

p（t）=p1（t）-c*b（t）*p1（t）;

end

t=1:

plot（t,s,'r',t,Y,'g',t,x,'b'）;

用matlab做的kalman滤波程序,已通过测试

－－－－－－－－－－－－－－－－－－－－－－－－－－

还有下面一个Matlab源程序，显示效果更好。

clear

clc;

N=300;

CON=25;%房间温度，假定温度是恒定的

%%%%%%%%%%%%%%%kalmanfilter%%%%%%%%%%%%%%%%%%%%%%

x=zeros（1,N）;

y=2^0.5*randn（1,N）+CON;%加过程噪声的状态输出

（1）=1;

p=10;

Q=cov（randn（1,N））;%过程噪声协方差

R=cov（randn（1,N））;%观测噪声协方差

fork=2:

x（k）=x（k-1）;%预估计k时刻状态变量的值

p=p+Q;%对应于预估值的协方差

kg=p/（p+R）;%kalmangain

x（k）=x（k）+kg*（y（k）-x（k））;

p=（1-kg）*p;

end

%%%%%%%%%%%SmoothnessFilter%%%%%%%%%%%%%%%%%%%%%%%%

Filter_Wid=10;

smooth_res=zeros（1,N）;

fori=Filter_Wid+1:

tempsum=0;

forj=i-Filter_Wid:

i-1

tempsum=tempsum+y（j）;

end

smooth_res（i）=tempsum/Filter_Wid;

end

%figure

（1）;

%hist（y）;

t=1:

figure

（1）;

expValue=zeros（1,N）;

fori=1:

expValue（i）=CON;

end

plot（t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k'）;

legend（'expected','estimate','measure','smoothresult'）;

axis（[0N2030]）

xlabel（'Sampletime'）;

ylabel（'RoomTemperature'）;

title（'SmoothfilterVSkalmanfilter'）;

卡尔曼滤波算法--核心公式推导导论

再造红旗

写在最前面：

这是我第一篇专栏文章，感谢知乎提供这么一个平台，让自己能和大家分享知识。

本人会不定期的开始更新文章，文章的内容应该集中在汽车动力学控制，整车软件架构，控制器等方面。

作为一名在校硕士，很多理解都可能不全面，不正确，大家有不同意见欢迎讨论。

谢谢！

---------------------------------------------

卡尔曼滤波算法很牛逼，因为有一堆公式，有一堆符号，看起来就很牛逼啊，乍一看不懂的都很牛逼啊！

本文针对卡尔曼滤波算法的核心公式进行推导，不让大家被它华丽的外表吓到。

（之后计划写关于针对非线性情况的EKF和UKF，对卡尔曼滤波算法做一个全面一点的应用介绍。

感兴趣的可以关注专栏。

）

-------------------------------------------------------------------------------------------------------

Okay，进入正题。

这篇文章假设读者已经对卡尔曼滤波算法有初步的了解，知道它能做什么，知道它的优点，知道它很牛逼，并且你已经对它产生兴趣，但不知道如何下手。

首先给出一个控制理论中公式，别急着翻控制理论的书，没那么复杂：

两个基本问题：

1.卡尔曼滤波算法要做什么？

对状态进行估计。

2.卡尔曼滤波算法怎么对状态进行估计？

利用状态过程噪声和测量噪声对状态进行估计。

一个状态在一个时刻点k的状态进入下一个时刻点k+1状态，会有很多外界因素的干扰，我们把干扰就叫做过程噪声，（这个词一看就是硬翻译过来的，别在意为什么叫噪声）用w表示。

任何一个测量仪器，都会有误差，我们把这个误差叫做量测噪声，用v表示。

回到上面那个公式，状态方程表示状态在不断的更新，从一个时刻点进入下一个时刻点，这个很好理解。

关键是量测方程，它表示，我们不断更新的状态有几个能用测量仪器测出来，比如，汽车运动状态参数有很多，比如速度，轮速，滑移率等，但是我们只能测量出轮速，因此量测方程要做的就是把状态参数中能量测的状态拿出来。

我们始终要记得我们要做的事：

我们要得到的是优化的状态量Xk。

理解了上面之后就可以开始推导公式了。

1.首先不考虑过程噪声对状态进行更新，很简单：

举个例子，v（k）=v（k-1）+at，匀加速运动咯。

2.不考虑测量噪声取出能测量的状态，也很简单：

3.用测量仪器测量出来的状态值（大家可以考虑到：

测量的值就是被各种噪声干扰后的真实值）减去上面不考虑噪声得到的测量值：

这个值在数学上是一个定义值，叫做新息，有很多有趣的性质，感兴趣的可以自己谷歌。

我们对步骤暂且停一停。

这个叫新息的值有什么用？

由上面的过程我们可以明显看到，它反映了过程噪声和测量噪声综合对测量状态值的影响，也就是它包含了w和v的情况。

回到数学层面，（不要害怕，很简单的数学应用和思考啦！

）一个数值c由两部分内容a和b组成，那么怎样用数学表达式来表达？

一般有两种做法：

I.直接相加：

c=a+b;

II.用比例的方法：

a=n*c,b=（1-n）*c

卡尔曼采用了方法II，用比例的方法来做（其实这也是为什么叫做滤波的原因，因为滤波就是给权值之类的操作）。

也就是说，过程噪声w=新息*一个比例。

这样得到的过程噪声加上原来（第一步）不考虑过程噪声的状态值不就是优化值了吗？

也就是：

Okay，都写到这里了，有必要做一下前提假设：

a.什么高斯噪声，均值为零一堆；

b.Ak,Ck,wk的协方差Q,vk的协方差R，系统协方差初始值P0,状态初始值X0，都已知。

为什么已知，你实际做项目就知道了。

不过不懂的可以留言或者私信。

那么到目前为止我们的思路就是清楚了，找到一个合适的Hk值（卡尔曼增益），那么我们就能得到状态的最优值。

（卡尔曼说的，不是我说的，所以你问为什么，你要问他，这么深层次的理论留给博士和学者们去做就好，我们就现学现用就行，哈哈哈，站在巨人的肩膀！

）

问题来了：

怎么得到合适的Hk？

似乎不是随便一个参数。

这是误差协方差矩阵。

思路：

使得误差协方差矩阵Pk最小的Hk。

为什么？

这里我从感观的角度说明自己的理解，欢迎讨论。

协方差表示什么，协方差表示两者之间的联系或者关系，关系越大，协方差越大。

误差协方差越小说明过程噪声和量测噪声的关系越小。

关系越小能做什么，这要回到我们第3步讨论的我们用比例的方法分开了w和v。

用比例分开，到底多少属于w，多少是v，如果关系越小，分开的越精确，比如一堆白砂糖和盐，如果两种混合的很均匀，我们说它关系很大，也就越难用比例的方法将其分开。

4.求的误差协方差矩阵Pk

自然是把里面的Xk先得到，然后公式运算，通过上面的步骤我们也容易得到：

然后复杂的数学计算，和之前假设的高斯噪声，新息的性质之类（至于过程，个人觉得你如果只做应用，不研究算法，就没必要深入去看了），就能得到下面的卡尔曼滤波递推公式：

通过上面的解释，我们也就不难知道这些公式都在干嘛，知道干嘛就可以了。

在知道A,C,P0,Q,R的情况下，整个公式的运算流程也都很清晰了。

过程方程：

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行的单列矩阵。

说明：

下面的表达式中，不带前缀的量都代表实际量，其小括号里面的“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体现了这一点），这样预测值才最可靠。

下面详细说明。

^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）[i] = H[i] ~X（k+1|k） + V（k+1）[i] >>>>式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）[i]、H[i]~X（k+1|k）、V（k+1）[i] 三者之间的几何关系，也在下图中描绘了出来。

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

图2中（注意此图中的椭圆代表的是Vz空间，而图1中则代表Vv空间，二者不一样），粉色的向量就是Kg1*~Z（k+1|k）[1]+Kg2*~Z（k+1|k）[2]，记此粉色向量为 Y ，Y为~Z（k+1|k）[1]和~Z（k+1|k）[2]线性组合而成，它始终在子空间Vz中。

根据式9i，~X（k+1|k+1）[1]等于~X（k+1|k）[1]和Y的差向量，为使~X（k+1|k+1）[1]长度最短（协方差最小），Kg的选取应使得~X（k+1|k+1）[1]垂直于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中的元素，都代表测量仪器测到的偏差的可信度，或者叫估计权重。

附注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）） - （

展开阅读全文