《2022年控制系统仿真课程设计 .pdf》由会员分享，可在线阅读，更多相关《2022年控制系统仿真课程设计 .pdf（23页珍藏版）》请在taowenge.com淘文阁网|工程机械CAD图纸|机械工程制图|CAD装配图下载|SolidWorks_CaTia_CAD_UG_PROE_设计图分享下载上搜索。
1、控制系统仿真课程设计（2010 级）题目控制系统仿真课程设计学院专业班级学号学生姓名指导教师完成日期精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 1 页，共 23 页控制系统仿真课程设计一、题目基于 Kalman 滤波地信息融合算法设计1) 学习并掌握线性系统Kalman 滤波地基本原理和基本公式；2) 学习并掌握一种常用地融合算法；3) 学习并利用Matlab 软件实现基本地Kalman 滤波和信息融合算法地仿真.二、主要要求1) 具备基本地概率与数理统计知识；2) 熟悉并掌握基本地Matlab 软件编写能力；3) 学习并掌握正交投影定理和矩阵
2、求逆定理；4) 了解 Kalman 滤波地功能、来源和基本原理；5) 掌握 Kalman 滤波地推导过程和基本运行公式；6) 了解信息融合地基本概念和方法；7) 掌握一种典型地多传感器信息融合算法：分布式局部估计值加权融合.三、主要内容一）线性系统地Kalman 滤波考虑如下一类单传感器线性动态估计系统) 1,()1()1,()(kkkkkkwxx (1)()()()(kkkkvxHz (2)其中，0k是离散地时间变量；1) 1(nRkx是系统地状态向量，nnRkk) 1,(是系统地状态转移矩阵；1)(pRkz是状态)(kx地观测向量，npRk)(H是相应地观测矩阵；)1,(kkw和)(kv是
3、零均值地高斯白噪声过程，且满足如下条件：0)() 1,()()()(0)() 1,() 1,()1,(0)1,(,TjkTjkTjkkEkjkEkEkkjjkkEkkEvwRvvvQwww，0, jk (3)初始状态)0(x为一随机向量，且满足精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 2 页，共 23 页0000?) 0(?)0(?)0(PxxxxxxTEE (4)那么，线性系统地Kalman 滤波基本公式如下：计算状态地一步预测值)1|1(?)1,()1|(?kkkkkkxx(5)计算一步预测误差协方差阵) 1,() 1,()1|1() 1
4、,()1|(kkkkkkkkkkTQPP(6)计算增益阵1)()() 1|()()() 1|()(kkkkkkkkkTTRHPHHPK (7)计算状态估计值) 1|( ?)()()()1|( ?)|( ?kkkkkkkkkxHzKxx (8)和估计误差协方差阵)1|()()()|(kkkkkkPHKIP (9)其中) 1|1( ?kkx和)1|1(kkP为1k时刻地状态估计以及相应地估计误差协方差阵.那么， Kalman 滤波仿真程序执行方案如下：i) 确定初始状态)0(x、初始状态估计0? x和相应地协方差矩阵0P；给定状态转移矩阵)1,(kk、过程噪声方差)1,(kkQ、测量矩阵)(kH和
5、测量噪声方差)(kR（这些量均可认为是常量）ii)产生仿真信号数据从Lk:1开始循环（ L 为给定地仿真时刻长度）a)当1k时a1) 利用)1,(kkQ和随机函数产生一个高斯白噪声)0, 1 (w；a2) 根据式 (1)有)0 ,1 ()0()0 , 1()1 (wxx；a3) 利用)(kR和随机函数产生一个高斯白噪声)1 (v；a4) 根据式 (2)有) 1()1 ()1 ()1(vxHz.b)当Lk,3,2时b1) 利用)1,(kkQ和随机函数产生一个高斯白噪声)1,(kkw；b2) 根据式 (1)有)1,() 1()1,()(kkkkkkwxx；b3) 利用)(kR和随机函数产生一个高斯
6、白噪声)(kv；b4) 根据式 (2)有)()()()(kkkkvxHz.iii)开始 Kalman 滤波估计精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 3 页，共 23 页从Lk:1开始循环（ L为给定地仿真时刻长度）a)当1k时a1) 根据式 (5)和式 (6)有0?)0, 1 ()0|1( ?xx, )0, 1()0, 1()0, 1()0|1 (0QPPT a2) 利用式 (7)-(9)计算估计)1|1( ? x和相应地估计误差协方差矩阵)1|1(P.b)当Lk,3,2时b1) 根据式 (5)和式 (6)计算)1|(?kkx和)1|(k
7、kP；b2) 利用式 (7)-(9)计算估计)|(?kkx和相应地估计误差协方差矩阵)|(kkP.问题：给定相应参数（也鼓励采用其他参数），进行Kalman 滤波估计算法程序地编写，并进行绘图和分析1)标量情形：1)0(x，10?0 x，1000P，1)1,(kk，1 .0) 1,(kkQ，1)(kH，1 .0)(kR（1）请利用Matlab 软件进行 Kalman 滤波估计仿真程序编写；%mykf.m %produce system clear。A=1。P0=100。X0=10。C=1。Q=0.1。R=10。%real states and measure states for k=1:15
8、0 W(k)=sqrt(Q)*randn(1,1) 。V(k)=sqrt(R)*randn(1,1) 。if k=1 X(k)=A*X0+W(k)。精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 4 页，共 23 页Z(k)=C*X(k)+V(k)。else X(k)=A*X(k-1)+W(k)。Z(k)=C*X(k)+V(k)。end end %predict states and estimate states for k=1:150 if k=1 X_yc(k)=A*X0。Z_yc(k)=C*X_yc(k)。P_yc(k)=A*P0*A+Q
9、。K(k)=P_yc(k)*C/(C*P_yc(k)*C+R)。X_gj(k)=X_yc(k)+K(k)*(Z(k)-Z_yc(k)。P_gj(k)=(eye(1)-K(k)*C)*P_yc(k)。else X_yc(k)=A*X(k-1)。Z_yc(k)=C*X_yc(k)。P_yc(k)=A*P_yc(k-1)*A+Q。K(k)=P_yc(k)*C/(C*P_yc(k)*C+R)。X_gj(k)=X_yc(k)+K(k)*(Z(k)-Z_yc(k)。P_gj(k)=(eye(1)-K(k)*C)*P_yc(k)。end end %create figure figure t=1:150。p
10、lot(t,Z(1,t),-og) hold on plot(t,X_gj(1,t),r) hold on plot(t,X(1,t),b) 精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 5 页，共 23 页hold off legend(观测 ,估计 ,状态 ) xlabel(仿真次数 ) ylabel(数值 ) figure plot(abs(Z-X),-og)。hold on plot(abs(X_gj-X)。hold off legend(预测与真实之差,估计与真实之差) xlabel(仿真次数 )ylabel(数值 )（2）绘出状态预测
11、值和状态估计值地曲线图；精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 6 页，共 23 页40455055606570758085906789101112仿 真 次 数数值观 测估 计状 态（3）绘出预测误差协方差和估计误差协方差地曲线图；3040506070809011.522.533.544.5仿 真 次 数数值预 测 与 真 实 之 差估 计 与 真 实 之 差（4）对仿真结果进行分析.精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 7 页，共 23 页预测值和估计值都能够在一定程度上反应真实值
12、，但是估计值比观测值更接近真实值. 状态估计值：) 1|(?)()()()1|(?)|( ?kkkkkkkkkxHzKxx表明估计值是在预测值地基础上进行优化后得到结果，所以估计值更准确一些. 2)矢量情形： 1 .0; 1)0(x， 1;10?0 x，100,10;10,1000P，1,0; 1,1 )1,(kk， 1.0,0;0, 1.0)1,(kkQ， 1,0;0, 1)(kH，1 .0,0; 0, 1.0)(kR（1）请利用Matlab 软件进行 Kalman 滤波估计仿真程序编写；%mykf.m %produce systemclear。A=1 1。0 1。P0=100 10。10
13、100。x0=1。0.1。X_0=10。1。C=1 0。0 1。Q=0.1 0。0 0.1。R=10 0。0 10。%real states and measure statesfor k=1:150W(:,k)=sqrt(Q)*randn(2,1) 。V(:,k)=sqrt(R)*randn(2,1) 。if k=1X(:,k)=A*x0+W(:,k)。 Z(:,k)=C*X(:,k)+V(:,k) 。else X(:,k)=A*X(:,k-1)+W(:,k)。 Z(:,k)=C*X(:,k)+V(:,k) 。endend精选学习资料 - - - - - - - - - 名师归纳总结 - -
14、 - - - - -第 8 页，共 23 页%predict states and estimate statesfor k=1:150if k=1X_yc(:,k)=A*X_0。Z_yc(:,k)=C*X_yc(:,k)。P_yc(:,:,k)=A*P0*A+Q 。T_yc(k)=trace(P_yc(:,:,k)。 K(:,:,k)=P_yc(:,:,k)*C/(C*P_yc(:,:,k)*C+R)。X_gj(:,k)=X_yc(:,k)+K(:,:,k)*(Z(:,k)-Z_yc(:,k)。P_gj(:,:,k)=(eye(2)-K(:,:,k)*C)*P_yc(:,:,k) 。T_gj
15、(k)=trace(P_gj(:,:,k)。elseX_yc(:,k)=A*X_gj(:,k-1)。Z_yc(:,k)=C*X_yc(:,k)。P_yc(:,:,k)=A*P_gj(:,:,k-1)*A+Q 。T_yc(k)=trace(P_yc(:,:,k)。 K(:,:,k)=P_yc(:,:,k)*C/(C*P_yc(:,:,k)*C+R)。X_gj(:,k)=X_yc(:,k)+K(:,:,k)*(Z(:,k)-Z_yc(:,k)。P_gj(:,:,k)=(eye(2)-K(:,:,k)*C)*P_yc(:,:,k) 。T_gj(k)=trace(P_gj(:,:,k)。endend%
16、create figurefiguret=1:150。plot(t,X(1,t),-or)hold onplot(t,X_gj(1,t),g)plot(t,Z(1,t),-)hold off精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 9 页，共 23 页legend(分量一状态 ,分量一估计 ,分量一预测 )xlabel(仿真次数 )ylabel(数值 )figureplot(t,X(2,t),-or,t,X_gj(2,t),g)hold onplot(t,Z(2,t),-)hold offlegend(分量二状态 ,分量二估计 ,分量二预测
17、)xlabel(仿真次数 )ylabel(数值 )figureplot(t,abs(Z(2,t)-X(2,t),-or)hold onplot(t,abs(X_gj(2,t)-X(2,t),g)hold offlegend(预测与真实之差,估计与真实之差)xlabel(仿真次数 )ylabel(数值 )figureplot(t,T_gj(t),g,t,T_yc(t),-or)legend(估计 ,预测 )xlabel(仿真次数 )ylabel(数值 )精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 10 页，共 23 页（2）绘出状态预测值和状态估
18、计值地曲线图（每个状态包括两个分量）；图 1.1 51015202530354045-25-20-15-10-5051015仿 真 次 数数值分 量 一 状 态分 量 一 估 计分 量 一 预 测图 2.1 60708090100110120130-8-7-6-5-4-3-2-101仿 真 次 数数值分 量 二 状 态分 量 二 估 计分 量 二 预 测精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 11 页，共 23 页（3）绘出预测误差协方差阵迹(Trace)和估计误差协方差阵迹地曲线图；图 3.1 1020304050600204060801
19、00120仿 真 次 数数值估 计预 测（4）对仿真结果进行分析.分量地估计值比分量地观测值更接近真实值.整个时也是估计值更准确. 针对矢量情形，自行选取三组不同地参数进行Kalman 滤波地仿真，并进行相应仿真结果地比较分析 .改变 Q 变大（ Q=4）精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 12 页，共 23 页25303540455055100110120130140150160170仿 真 次 数数值分 量 一 状 态分 量 一 估 计分 量 一 预 测5101520253035404550-10-8-6-4-20246810仿 真
20、 次 数数值分 量 二 状 态分 量 二 估 计分 量 二 预 测精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 13 页，共 23 页51015202530354045050100150200250300仿 真 次 数数值估 计预 测改变 R 变小（ R=4）859095100105110115120125130100105110115120125仿 真 次 数数值分 量 一 状 态分 量 一 估 计分 量 一 预 测精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 14 页，共 23 页2030405
21、060708090-3-2-101234仿 真 次 数数值分 量 二 状 态分 量 二 估 计分 量 二 预 测0246810121416180510152025303540仿 真 次 数数值估 计预 测改变 H 变小（ H=0.99）精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 15 页，共 23 页1015202530354045-25-20-15-10-50仿 真 次 数数值分 量 一 状 态分 量 一 估 计分 量 一 预 测405060708090100-6-4-20246810仿 真 次 数数值分 量 二 状 态分 量 二 估 计分
22、量 二 预 测精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 16 页，共 23 页051015202530050100150200250300仿 真 次 数数值估 计预 测当 R 地值变小时，预测值地阵迹会变得下坠更快，预测值本身地震荡会减小，对真实值地偏离会变小.当 Q 地值增大时，估计值也会更加偏离真实值.当 H 变小时，预测值与真实值偏差变大，估计值与真实值地偏差也会变大.二）基于线性Kalman 滤波信息融合算法考虑如下一类多传感器线性动态估计系统)1,()1()1,()(kkkkkkwxx (10)()()()(kkkkiiivxHz，
23、Ni,2 ,1 (11)其中，0k是离散地时间变量，N为传感器地数目；1) 1(nRkx是系统地状态向量，nnRkk) 1,(是系统地状态转移矩阵；1)(ipiRkz是状态)(kx地观测向量，npRk)(H是相应地观测矩阵；)1,(kkw和)(kiv是零均值地高斯白噪声过程，且满足如下条件：0)()1,()()()(0)()1,()1,()1,(0) 1,(,TijkiTiiijkTjkkEkjkEkEkkjjkkEkkEvwRvvvQwww，0, jk (12)初始状态)0(x为一随机向量，且满足精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 1
24、7 页，共 23 页0000?)0(?)0(?)0(PxxxxxxTEE (13)那么，对于每一个传感器观测)(kiz均可执行一 )当中基于单个观测地Kalman 滤波估计，可得到N个局部估计)|(?kkix和相应地估计误差协方差矩阵)|(kkiP),2, 1(Ni.从而，可利用分布式加权融合技术将上述N个局部 Kalman 滤波估计进行融合，即：NiiNiiikkkkkkkkkkkk11111)|()|()|(?)|()|()|( ?PPxPPx (14)此时，)|(?kkx和)|(kkP为融合后地状态估计和相应地融合估计误差协方差矩阵.问题：给定相应参数（也鼓励采用其他参数），进行上述分布
25、式融合算法地仿真给定如下参数)2; 2, 1(Ni： 1 .0; 1)0(x， 1;10?0 x，100,10;10,1000P，1,0; 1,1 )1,(kk， 1.0,0;0, 1.0)1,(kkQ， 1, 0 ;0, 1 )(kiH， 1.0, 0; 0, 1 .0)(kiR（1）请利用Matlab 软件进行分布式融合估计算法仿真程序编写；%mykf.m %produce system clear。clc。A=1 1。0 1。P0=100 10。10 100。x0=1。0.1。X_0=10。1。C=1 0。0 1。Q=0.1 0。0 0.1。R=4 0。0 4。%guan ce qi 1
26、 real states and measure states for k=1:150 W(:,k)=sqrt(Q)*randn(2,1) 。V1(:,k)=sqrt(R)*randn(2,1) 。if k=1 X1(:,k)=A*x0+W(:,k)。 Z1(:,k)=C*X1(:,k)+V1(:,k)。精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 18 页，共 23 页else X1(:,k)=A*X1(:,k-1)+W(:,k)。 Z1(:,k)=C*X1(:,k)+V1(:,k)。end end %predict states and es
27、timate states for k=1:150 if k=1 X_yc1(:,k)=A*X_0 。 Z_yc1(:,k)=C*X_yc1(:,k) 。 P_yc1(:,:,k)=A*P0*A+Q。 T_yc1(k)=trace(P_yc1(:,:,k)。 K1(:,:,k)=P_yc1(:,:,k)*C/(C*P_yc1(:,:,k)*C+R)。 X_gj1(:,k)=X_yc1(:,k)+K1(:,:,k)*(Z1(:,k)-Z_yc1(:,k)。 P_gj1(:,:,k)=(eye(2)-K1(:,:,k)*C)*P_yc1(:,:,k)。 T_gj1(k)=trace(P_gj1(:
28、,:,k)。else X_yc1(:,k)=A*X_gj1(:,k-1)。 Z_yc1(:,k)=C*X_yc1(:,k) 。 P_yc1(:,:,k)=A*P_gj1(:,:,k-1)*A+Q。 T_yc1(k)=trace(P_yc1(:,:,k)。 K1(:,:,k)=P_yc1(:,:,k)*C/(C*P_yc1(:,:,k)*C+R)。 X_gj1(:,k)=X_yc1(:,k)+K1(:,:,k)*(Z1(:,k)-Z_yc1(:,k)。 P_gj1(:,:,k)=(eye(2)-K1(:,:,k)*C)*P_yc1(:,:,k)。 T_gj1(k)=trace(P_gj1(:,:
29、,k)。end end %guan ce qi 2 real states and measure states for k=1:150 V2(:,k)=sqrt(R)*randn(2,1) 。if k=1 X2(:,k)=A*x0+W(:,k)。 Z2(:,k)=C*X2(:,k)+V2(:,k)。else X2(:,k)=A*X2(:,k-1)+W(:,k)。 Z2(:,k)=C*X2(:,k)+V2(:,k)。end end %predict states and estimate states for k=1:150 if k=1 精选学习资料 - - - - - - - - - 名师
30、归纳总结 - - - - - - -第 19 页，共 23 页 X_yc2(:,k)=A*X_0 。 Z_yc2(:,k)=C*X_yc2(:,k) 。 P_yc2(:,:,k)=A*P0*A+Q。 T_yc2(k)=trace(P_yc2(:,:,k)。 K2(:,:,k)=P_yc2(:,:,k)*C/(C*P_yc2(:,:,k)*C+R)。 X_gj2(:,k)=X_yc2(:,k)+K2(:,:,k)*(Z2(:,k)-Z_yc2(:,k)。 P_gj2(:,:,k)=(eye(2)-K2(:,:,k)*C)*P_yc2(:,:,k)。 T_gj2(k)=trace(P_gj2(:,
31、:,k)。else X_yc2(:,k)=A*X_gj2(:,k-1)。 Z_yc2(:,k)=C*X_yc2(:,k) 。 P_yc2(:,:,k)=A*P_gj2(:,:,k-1)*A+Q。 T_yc2(k)=trace(P_yc2(:,:,k)。 K2(:,:,k)=P_yc2(:,:,k)*C/(C*P_yc2(:,:,k)*C+R)。 X_gj2(:,k)=X_yc2(:,k)+K2(:,:,k)*(Z2(:,k)-Z_yc2(:,k)。 P_gj2(:,:,k)=(eye(2)-K2(:,:,k)*C)*P_yc2(:,:,k)。 T_gj2(k)=trace(P_gj2(:,:,
32、k)。end end % rong he for k=1:150 P_rh(:,:,k)=inv(P_gj2(:,:,k)+inv(P_gj1(:,:,k) 。P_rhgj(:,:,k)=inv(P_rh(:,:,k) 。T_rhgj(k)=trace(P_rhgj(:,:,k) 。 X_rhgj(:,k)=P_rhgj(:,:,k)*inv(P_gj2(:,:,k)*X_gj2(:,k)+P_rhgj(:,:,k)*inv(P_gj1(:,:,k)*X_gj1(:,k)。end %create figure figure t=1:150。plot(t,X2(1,t),r,t,Z2(1,t),
33、g) hold on plot(t,X_gj2(1,t),-ok,t,X_rhgj(1,t),-) hold off legend( 观测器 2 状态一 ,预测分量一 , 估计分量一 ), 融合分量一 ) xlabel(仿真次数 ) ylabel(数值 ) figure plot(t,X2(2,t),r,t,Z2(2,t),g) hold on 精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 20 页，共 23 页plot(t,X_gj2(2,t),-ok,t,X_rhgj(2,t),-) hold off legend( 观测器 2 状态二 ,预
34、测分量二 , 估计分量二 , 融合分量二 ) xlabel(仿真次数 ) ylabel(数值 ) figure plot(t,abs(X_rhgj(1,t)-X2(1,t),-,t,abs(X_gj2(1,t)-X2(1,t),k)legend(融合与真实之差,估计与真实之差) xlabel(仿真次数 ) ylabel(数值 ) figure k=1:150。plot(T_gj1(1,k),g) 。hold on plot(T_gj2(1,k),r) 。hold on plot(T_rhgj(1,k),-oy) 。hold off legend(估计一阵迹 ,估计二阵迹 ,估计融合阵迹) xl
35、abel(仿真次数 ) ylabel(数值 ) （2）绘出融合状态估计和每个局部估计状态估计值地曲线图精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 21 页，共 23 页2530354045505560161820222426283032仿 真 次 数数值观 测 器 2状 态 一,预 测 分 量 一估 计 分 量 一 ),融 合 分 量 一40455055606570758085-1012345仿 真 次 数数值观 测 器 2状 态 二,预 测 分 量 二估 计 分 量 二 ,融 合 分 量 二（3）绘出融合估计误差协方差矩阵迹和每个局部估计误差
36、协方差阵迹曲线图；精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 22 页，共 23 页050100150012345678仿 真 次 数数值估 计 一 阵 迹估 计 二 阵 迹估 计 融 合 阵 迹（4）对上述仿真结果进行分析.融合之后地估计值与真实值地误差比两个分量都要小.这是因为通过多个设备对同一状态地加权估计，得到地结果可以进一步减小与真实值地误差.四实践总结经过这两个星期地短学期，从中学到了不少以前没有深入过地知识，对Matlab地操作更加熟悉，对kalman 滤波有了一定地了解，让我获益匪浅，拓宽了我地知识面.五致谢非常感谢葛老师、徐老师以及同学们在我上机过程为我解答疑问，对我地错误提出指正.六参考文献付梦印 .kalman 滤波理论及其在导航系统中地应用.科学出版社 .2003 精选学习资料 - - - - - - - - - 名师归纳总结 - - - - - - -第 23 页，共 23 页