卡尔曼滤波算法与matlab实现文档格式.docx
- 文档编号:5843179
- 上传时间:2023-05-05
- 格式:DOCX
- 页数:28
- 大小:542.04KB
卡尔曼滤波算法与matlab实现文档格式.docx
《卡尔曼滤波算法与matlab实现文档格式.docx》由会员分享,可在线阅读,更多相关《卡尔曼滤波算法与matlab实现文档格式.docx(28页珍藏版)》请在冰点文库上搜索。
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,就是卡尔曼增益〔KalmanGain〕。
他可以随不同的时刻而改变他自己的值,是不是很神奇!
下面就要言归正传,讨论真正工程系统上的卡尔曼。
3.卡尔曼滤波器算法
〔TheKalmanFilterAlgorithm〕
在这一局部,我们就来描述源于DrKalman的卡尔曼滤波器。
下面的描述,会涉及一些根本的概念知识,包括概率〔Probability〕,随即变量〔RandomVariable〕,高斯或正态分配〔GaussianDistribution〕还有State-spaceModel等等。
但对于卡尔曼滤波器的详细证明,这里不能一一描述。
首先,我们先要引入一个离散控制过程的系统。
该系统可用一个线性随机微分方程〔LinearStochasticDifferenceequation〕来描述:
X(k)=AX(k-1)+BU(k)+W(k)
再加上系统的测量值:
Z(k)=HX(k)+V(k)
上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量。
A和B是系统参数,对于多模型系统,他们为矩阵。
Z(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵。
W(k)和V(k)分别表示过程和测量的噪声。
他们被假设成高斯白噪声(WhiteGaussianNoise),他们的covariance分别是Q,R〔这里我们假设他们不随系统状态变化而变化〕。
对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。
下面我们来用他们结合他们的covariances来估算系统的最优化输出〔类似上一节那个温度的例子〕。
首先我们要利用系统的过程模型,来预测下一状态的系统。
假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态:
X(k|k-1)=AX(k-1|k-1)+BU(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)=AP(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)-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;
w
(1)=0;
w=randn(1,N)
x
(1)=0;
a=1;
fork=2:
N;
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;
p
(1)=0;
s
(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);
t=1:
plot(t,s,'
r'
t,Y,'
g'
t,x,'
b'
);
用matlab做的kalman滤波程序,已通过测试
--------------------------
还有下面一个Matlab源程序,显示效果更好。
clc;
N=300;
CON=25;
%房间温度,假定温度是恒定的
%%%%%%%%%%%%%%%kalmanfilter%%%%%%%%%%%%%%%%%%%%%%
x=zeros(1,N);
y=2^0.5*randn(1,N)+CON;
%加过程噪声的状态输出
x
(1)=1;
p=10;
Q=cov(randn(1,N));
%过程噪声协方差
R=cov(randn(1,N));
%观测噪声协方差
fork=2:
N
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;
%%%%%%%%%%%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);
smooth_res(i)=tempsum/Filter_Wid;
%figure
(1);
%hist(y);
figure
(1);
expValue=zeros(1,N);
fori=1:
expValue(i)=CON;
plot(t,expValue,'
t,y,'
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,如果关系越小,分开的越精确,比方一堆白砂糖和盐,如果两种混合的很均匀,我们说它关系很大,也就越难用比例的方法将其分开。
自然是把里面的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为矩阵。
他们被假设成高斯白噪声,他们的协方差分别是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|k)
式4
并且
先验预测测量值
转换矩阵H
*先验预测状态值
^Z(k+1|k)
^X(k+1|k)
式5
得到测量值后,再对当前状态值X(k+1)进行后验预测〔设后验预测值为^Z(k+1|k+1)〕,那么后验预测值〔同时也是最终预测值〕的偏差为
~X(k+1|k+1)
^X(k+1|k+1)
式6
为了得到当前状态值X(k+1),根据式3,需要:
+
式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)
~X(k+1|k)
)-
^X(k+1|k)+
)
Kg*~Z(k+1|k)
式9
Z(k+1)
^Z(k+1|k)
)
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 卡尔 滤波 算法 matlab 实现