完整word版Kalman滤波MATLAB综合实验报告.docx
《完整word版Kalman滤波MATLAB综合实验报告.docx》由会员分享，可在线阅读，更多相关《完整word版Kalman滤波MATLAB综合实验报告.docx（10页珍藏版）》请在冰豆网上搜索。
完整word版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.42269
升级会员 