Kalman滤波MATLAB综合实验报告.docx
《Kalman滤波MATLAB综合实验报告.docx》由会员分享,可在线阅读,更多相关《Kalman滤波MATLAB综合实验报告.docx(12页珍藏版)》请在冰豆网上搜索。
Kalman滤波MATLAB综合实验报告
《数学实验》综合实验报告
实验名称 综合实验(Kalman滤波)
2016年5月
一、【实验目的】
明白滤波计算流程
能够调用相关函数进行数据处理
使用循环函数和二维曲线画图
有效的构建仿真模型,产生模拟数据
二、【实验原理分析】
卡尔曼滤波器是一个“optimalrecursivedataprocessingalgorithm(最优化自回归数据处理算法)”。
对于解决很大部分的问题,它是最优,效率最高甚至是最有用的。
它的广泛应用已经超过30年,包括机器人导航,控制,传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。
近来更被应用于计算机图像处理,例如头脸识别,图像分割,图像边缘检测等等。
设系统可用一个线性随机微分方程来描述:
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)分别表示过程和测量的噪声。
他们被假设成高斯白噪声,他们的协方差分别是Q,R(这里假设他们不随系统状态变化而变化)。
对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器。
首先要利用系统的过程模型,来预测下一状态的系统。
假设现在的系统状态是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)的协方差还没更新。
我们用P表示协方差:
P(k|k-1)=AP(k-1|k-1)A’+Q………
(2)
式
(2)中,P(k|k-1)是X(k|k-1)对应的协方差,P(k-1|k-1)是X(k-1|k-1)对应的协方差,A’表示A的转置矩阵,Q是系统过程的协方差。
式子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)的协方差:
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)。
这样,算法就可以自回归的运算下去。
、
MATLAB中已经给出了滤波函数,以下为直接调用方法:
设线性系统为
其调用格式为
[kest,L,P]=kalman(sys,Qn,Rn,Nn)
[kest,L,P]=kalman(sys,Qn,Rn,Nn,sensors,known)
[kest,L,P,M,Z]=kalman(sys,Qn,Rn,Nn)
最后一种调用格式只限于离散系统。
三、【实验内容及数据来源】
已知离散系统
第一式为系统方程,第二式为观测方程,
表示状态量x的第二个分量。
e与v是互不相关的高斯白噪声。
假设
的真值
,由此系统方程构造出k=1,2,…30的数据
构造时加上系统噪声干扰,再由观测方程构造出观测数据并加观测噪声干扰,并以此作为仿真数据。
用Kalman滤波对仿真数据进行滤波处理,并与真实结果比较。
四、【实验程序】
%%%%%%%系统描述
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%x[n+1]=Ax[n]+Bu[n]+Gw[n]
%y[n]=Cx[n]+Du[n]+Hw[n]+v[n]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%状态转移矩阵
A=[0.490.2980.412
0.401-0.3910.391
-0.9920.4010.296];
%B矩阵
B=zeros(3,3);
%G矩阵
G=eye(3,3);
%C矩阵向量
C=[010];
D=[000];
H=zeros(1,3);
%状态向量初值(真值)
x(:
1)=[10.98.481-4.3]';
%状态向量初始估计值
guji=[20.121.320.7]';
%进入循环
fori=2:
30
%c产生正态分布数据
w=randn(3,1);
v=randn(1,1);
%真实数据
x(:
i)=A*x(:
i-1);
%人为制造系统误差
x1(:
i)=x(:
i)+w;
Qn=eye(2,2);
Rn=1;
Nn=0;
%人为制造观测数据的误差
z0(:
i)=C*x1(:
i)+v;
%建立Kalman的系统参数
sys=ss(A,[B,G],C,[D,H],-1);
[kest,L,P,M,Z]=kalman(sys,Qn,Rn,Nn);
%得到估计数据
guji(:
i)=A*guji(:
i-1)+L*(z0(:
i)-C*A*guji(:
i-1));
end
subplot(2,2,1)
%做出真值曲线x1
plot(x(1,:
))
holdon
%做出在噪声污染情况下的滤波估计值曲线x1
plot(guji(1,:
),':
m')
holdoff
legend('realofx1','estimateofx1')
grid
subplot(2,2,2)
%做出真值曲线x2
plot(x(2,:
))
holdon
%做出在噪声污染情况下的滤波估计值曲线x2
plot(guji(2,:
),':
m')
holdoff
legend('realofx2','estimateofx2')
grid
subplot(2,2,3:
4)
%做出真值曲线x2
plot(x(3,:
))
holdon
%做出在噪声污染情况下的滤波估计值曲线x3
plot(guji(3,:
),':
m')
holdoff
legend('realofx1','estimateofx1')
grid
五、【实验结果】
为了阐述kalman滤波的基本计算思路,在试验中选择了最简单的一种系统模型,线性定常动力系统。
在实际应用中很多时候面对的都是非线性而且时变系统,这有时候性质要复杂一些。
最后的结果信息我们反映在图9.2中,这只是结果的直观反应,更多的时候需要的是计算结果。
表9.1中给出了真实的运动轨迹。
图1.1Kalman滤波处理的与真实值的比较
表1.1真实的运动状态数据
时间序列
第一个分量
第二个分量
第三个分量
1
10.9
8.481
-4.3
2
6.096738
-0.626471
-8.684719
3
-0.777390966
-0.70598303
-8.869855791
4
-4.245685102
-3.503808027
-2.137404671
5
-4.005131216
-1.168256014
2.17402082
6
-1.41495685102
-0.299227376
4.148129668
7
0.92653024
1.171518442
2.51149455
8
1.837848068
0.895469285
0.294063284
9
1.288549473
0.501827329
-1.377019368
10
0.213601806
-0.21792072
-1.484606051
11
-0.571933183
-0.40961964
-0.738722592
12
-0.70666762
-0.35802446
0.184438354
13
-0.376969821
-0.071270755
0.612040224
14
0.046206675
0.116009694
0.526538396
15
0.274145979
0.179045599
0.156538231
16
0.252180869
0.101132157
-0.153820209
17
0.090332082
0.001438153
-0.255140209
18
-0.060426476
-0.064098975
-0.164554228
19
-0.11650681
-0.063509021
-0.014468676
20
-0.08197512
-0.027544456
0.08582491
21
-0.013016194
0.011455399
0.095678165
22
0.036455178
0.027711608
0.045826416
23
0.04500158
0.021701416
-0.011486563
24
0.023785332
0.005069134
-0.039339322
25
-0.003042386
-0.007825788
-0.033206766
26
-0.017504042
-0.011143959
-0.009949297
27
-0.015996991
-0.006552008
0.00995029
28
-0.005691504
3.76E-05
0.016186945
29
0.003891391
0.004032099
0.010452388
30
0.007414731
0.004070781
0.000850519
表1.2给出了三个分量的滤波处理结果。
如果对比表1.1与表1.2,可以看出在序列靠后的地方真实状态与估计状态的绝对误差是比较小的。
表1.2Kalman滤波处理结果
时间序列
第一个分量
第二个分量
第三个分量
1
20.1
21.3
20.7
2
22.7252488
9.494219162
-6.316169216
3
10.40651623
3.728682279
-21.10563016
4
-1.788182459
-6.118879723
-14.71090286
5
-8.614322881
-4.198556849
-4.957777626
6
-6.763631785
-4.378077639
5.787030676
7
-2.357407994
1.364838315
6.60265909
8
1.422692466
1.56099626
4.553088326
9
2.392673342
2.2791037
0.224864281
10
1.861692101
0.225133574
-1.436205036
11
0.517263742
-0.01125328
-2.113843039
12
-0.58022756
-0.648547414
-1.22123626
13
-0.593150559
-0.707212838
0.164664887
14
-0.553204215
0.202906809
0.290993037
15
-0.391235191
0.063405251
0.559150432
16
-0.618625591
0.6001259193
0.22549466
17
-0.618625591
-0.783860719
1.165157579
18
-0.045343222
1.357835159
-0.664868843
19
-0.11295527
-0.624252831
0.276887725
20
-0.398859191
0.53368178
-0.198301503
21
-0.179721431
-0.394725491
0.518760826
22
-0.279207237
0.524824267
0.023365338
23
0.448919666
-0.658297751
0.7137889
24
0.479427729
0.581682628
-0.413558092
25
0.032585161
-0.025564549
-0.472086999
26
0.014970698
-0.329369401
-0.077156563
27
-0.852877938
0.7140655
-0.551591054
28
0.072338102
-1.258081817
1.233014025
29
-0.453268751
1.521953811
-0.536391476
30
-0.003343612
-0.975064648
0.893963618
六、【实验心得】
MATLAB中有丰富的图形处理能力,提供了绘制各种图形、图像数据的函数。
它可以提供了一组绘制二维和三维曲线的函数,他们还可以对图形进行旋转、缩放等操作。
MATLAB内部还包含丰富的数学函数和数据类型,使用方便且功能非常强大。
本学期通过对MATLAB的系统环境,数据的各种运算,矩阵的分析和处理,程序设计,绘图,数值计算及符号运算的学习,初步掌握了MATLAB的实用方法。
通过老师的讲解与自己上机的操作,使我在短时间内学会使用MATLAB,同时,通过上机实验,对理论知识的复习巩固实践,可以自己根据例题编写设计简单的程序来实现不同的功能,绘制出比较满意的二维三维图形,在实践中找到乐趣。
MATLAB是一个实用性很强,操作相对容易,比较完善的工具软件,使用起来比较方便,通过操作可以很快看到结果,能够清晰的感觉到成功与失败,虽然课程中也会出现一些小问题,但是很喜欢这门课程。
——郑蕊
MATLAB作为一门编程语言,其简易程度超过了大多数的计算机高级语言,作为一名计算机专业的学生,我深深感受到了这门语言的简洁与方便,语法和自然语言更为贴近,简便易学,功能强大,能够用其进行绝大多少的数学计算与绘图。
而且我们的数值计算就是用MATLAB作为我们的计算的工具,所以感觉这门课程多我的帮助很大,而且可以将其接入java等多种主流编程语言程序运行,兼容性很好。
用它进行数值分析计算,大大的方便了我们的任务的进行。
总之,通过这学期为数不多的学习,我掌握了MATLAB的许多基础入门知识,我以后继续学习熟练使用它进行问题分析解决打下了良好的基础。
俗话说,万事开头难,感谢这门课称帮助我踏入数学问题分析计算的新高度。
——周其飞
刚开始看到“数学实验”这门课时,我的内心是疑惑的,数学,还能有什么实验?
在进入课堂之后才感受到这门课深重的意义。
通过matlab实验使我学习掌握了许多知识。
首先是对matlab与数学实验有了一个全新的认识,其次是对matlab的更多操作和命令的使用有了更好的掌握,最重要的事对matlab的处理能力有了一个更高的飞跃尤其是对相关函数的使用及相关问题的处理。
matlab功能强大而又简单容易上手,通过短短几周的学习我们便掌握了它的基本操作及命令。
利用matlab进行矩阵和数组的运算使得原本复杂的矩阵运算化身为几行代码,而利用matlab的绘图功能更是将原本抽象的复杂函数图像显现出来并自主加以修饰,让人一目了然。
matlab在求导、积分方面也颇有功用。
数学本身是一门科学,其目的不是在于计算,而在于深入的探索与发现。
利用matlab软件处理数学问题,将我们从枯燥复杂的计算工作中解放出来,大大提高了我们实验研究的效率。
matlab带给我的影响远不止于此,在接下来的学习中的每一门课,matlab将成为一个必备的工具。
——赵冠晨
【参考文献】:
(1)《OptimalStateEstimation》,作者:
DanSimon
(2)《卡尔曼滤波与组合导航原理》,作者:
秦永元
(3)《数值计算方法》,主编:
颜兵兵