控制系统仿真课程设计

1、控制系统仿真课程设计（2010级）控制系统仿真课程设计题目学院专业班级学号学生姓名指导教师完成日期(3)控制系统仿真课程设计、题目基于Kalman滤波地信息融合算法设计学习并掌握线性系统Kalman滤波地基本原理和基本公式；学习并掌握一种常用地融合算法；学习并利用Matlab软件实现基本地Kalman滤波和信息融合算法地仿真.二、主要要求具备基本地概率与数理统计知识；熟悉并掌握基本地Matlab软件编写能力；学习并掌握正交投影定理和矩阵求逆定理；了解Kalman滤波地功能、来源和基本原理；掌握Kalman滤波地推导过程和基本运行公式；了解信息融合地基本概念和方法；掌握一种典型地多传感器信息融合

2、算法：分布式局部估计值加权融合.三、主要内容一)线性系统地Kalman滤波考虑如下一类单传感器线性动态估计系统TOC o 1-5 h zx(k)=(k,k-l)x(k-1)+w(k,k-1)z(k)二H(k)x(k)+v(k)(2)其中，k0是离散地时间变量；x(k一1)GRnx1是系统地状态向量，伙,k一1)GRnxn是系统地状态转移矩阵；Z(k)GRpx1是状态x(k)地观测向量，H(k)GRpxn是相应地观测矩阵；w(k,k-1)和v(k)是零均值地高斯白噪声过程，且满足如下条件：Ew(k,k-1)=0.EW(k,k-1)w(j,j-1)t/=Q(k,k-1)J0E”)v(j)t)=R(

3、f)、EW(k,k-1)v(j)t/=0初始状态x(0)为一随机向量，且满足(4)E*(0)=Xe1x(0)-XX(0)-X00那么，线性系统地Kalman滤波基本公式如下：计算状态地一步预测值X(kIk-1)二(k,k-1)X(k-11k-1)计算一步预测误差协方差阵P(kIk1)二G(k,k1)P(k1Ik1)G(k,k1)T+Q(k,k1)TOC o 1-5 h z计算增益阵_K(k)二P(kIk1)Ht(k)H(k)P(kIk1)Ht(k)+R(k)】1计算状态估计值X(kIk)=X(kIk1)+K(k)lz(k)H(k)X(kIk1)和估计误差协方差阵P(kIk)=IK(k)H(k)

4、b(kIk1)其中X(k1Ik1)和P(k1Ik1)为k1时刻地状态估计以及相应地估计误差协方差阵.那么，Kalman滤波仿真程序执行方案如下：确定初始状态x(0)、初始状态估计X和相应地协方差矩阵P；给定状态转移矩阵000(k,k1)、过程噪声方差Q(k,k1)、测量矩阵H(k)和测量噪声方差R(k)(这些量均可认为是常量)ii)产生仿真信号数据从k=1:L开始循环(L为给定地仿真时刻长度)当k=1时al)利用Q(k,k1)和随机函数产生一个高斯白噪声w(1,0)；a2)根据式有x(1)=0(1,0)x(0)+w(1,0)；a3)利用R(k)和随机函数产生一个高斯白噪声v(1)；a4)根据式

5、(2)有z(1)=H(1)x(1)+v(1).当k=2,3,L时b1)利用Q(k,k1)和随机函数产生一个高斯白噪声w(k,k1)；b2)根据式有x(k)=O(k,k1)X(k1)+w(k,k1)；b3)利用R(k)和随机函数产生一个高斯白噪声v(k)；b4)根据式有z(k)=H(k)X(k)+v(k).iii)开始Kalman滤波估计从k=1:L开始循环(L为给定地仿真时刻长度)当k=1时a1)根据式(5)和式(6)有X(1丨0)=(1,O)X,P(110)=(l,0)P0(1,0)T+Q(1,0)00a2)利用式(7)-(9)计算估计X(111)和相应地估计误差协方差矩阵P(111).当k

6、=2,3,L时b1)根据式和式(6)计算X(kIk-1)和P(kIk-1)；b2)利用式(7)-(9)计算估计X(kIk)和相应地估计误差协方差矩阵P(kIk).问题：给定相应参数(也鼓励采用其他参数)，进行Kalman滤波估计算法程序地编写，并进行绘图和分析1)标量情形：x(0)=1，X=10，P=100，0(k,k-1)=1，Q(k,k-1)=0.1，00H(k)=1，R(k)=0.1(1)请利用Matlab软件进行Kalman滤波估计仿真程序编写；%mykf.m%producesystemclear。A=1。P0=100。X0=10。C=1。Q=0.1。R=10。%realstatesa

7、ndmeasurestatesfork=1:150W(k)=sqrt(Q)*randn(1,1)。V(k)=sqrt(R)*randn(1,1)。ifk=1X(k)=A*X0+W(k)。Z(k)=C*X(k)+V(k)。elseX(k)=A*X(k-1)+W(k)。Z(k)=C*X(k)+V(k)。endend%predictstatesandestimatestatesfork=1:150ifk=1X_yc(k)=A*X0。Z_yc(k)=C*X_yc(k)。P_yc(k)=A*P0*A+Q。K(k)=P_yc(k)*C/(C*P_yc(k)*C+R)。X_gj(k)=X_yc(k)+K(k

8、)*(Z(k)-Z_yc(k)P_gj(k)=(eye(1)-K(k)*C)*P_yc(k)。elseX_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)。endend%createfigurefiguret=1:150。plot(t,Z(1,t),-og)holdonplot(t,X_gj(1,t),r)holdonplot(t,X(1,

9、t),b)holdoffIegend（观测,估计,状态）xlabel（仿真次数）ylabel（数值）figureplot（abs（Z-X）,-og）。holdonplot（abs（X_gj-X）。holdofflegend（预测与真实之差,估计与真实之差）xlabel（仿真次数）ylabel（数值）2）绘出状态预测值和状态估计值地曲线图；4045505560657075808590仿真次数4.543.532.521.513）绘出预测误差协方差和估计误差协方差地曲线图；3040预测与真实之差4估计与真实之差5060708090仿真次数4）对仿真结果进行分析.预测值和估计值都能够在一定程度上反应真

10、实值，但是估计值比观测值更接近真实值.状态估计值：X(kIk)二x(kIk-1)+K(k)lz(k)-H(k)x(kIk-1)表明估计值是在预测值地基础上进行优化后得到结果，所以估计值更准确一些.2)矢量情形：x(0)二1;0.1,x二10;1,P二100,10;10,100,00(k,k-1)二1,1;0,1,Q(k,k-1)二0.1,0;0,0.1,H(k)二1,0;0,1,R(k)二0.1,0;0,0.1(1)请利用Matlab软件进行Kalman滤波估计仿真程序编写；%mykf.m%producesystemclear。A=11。01。P0=10010。10100。xO=1。0.1。X

11、_0=10。1。C=10。01。Q=0.10。00.1。R=100。010。%realstatesandmeasurestatesfork=1:150W(:,k)=sqrt(Q)*randn(2,1)。V(:,k)=sqrt(R)*randn(2,1)。ifk=1X(:,k)=A*x0+W(:,k)。Z(:,k)=C*X(:,k)+V(:,k)。elseX(:,k)=A*X(:,k-1)+W(:,k)。Z(:,k)=C*X(:,k)+V(:,k)。endend%predictstatesandestimatestatesfork=1:150ifk=1X_yc(:,k)=A*X_0。Z_yc(:

12、,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(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)

13、*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%createfigurefiguret=1:150。plot(t,X(1,t),-or)holdonplot(t,X_gj(1,t),g)plot(t,Z(1,t),-)holdoffIegend(分量一

14、状态,分量一估计,分量一预测)xlabel(仿真次数)ylabel(数值)figureplot(t,X(2,t),-or,t,X_gj(2,t),g)holdonplot(t,Z(2,t),-)holdofflegend(分量二状态,分量二估计,分量二预测)xlabel(仿真次数)ylabel(数值)figureplot(t,abs(Z(2,t)-X(2,t),-or)holdonplot(t,abs(X_gj(2,t)-X(2,t),g)holdofflegend(预测与真实之差,估计与真实之差)xlabel(仿真次数)ylabel(数值)figureplot(t,T_gj(t),g,t,T

15、_yc(t),-or)legend(估计,预测)xlabel(仿真次数)ylabel(数值)（2）绘出状态预测值和状态估计值地曲线图（每个状态包括两个分量）图1.1151050值-5数-5-10-15分量一状态分量一估计分量一预测-20-2551015202530354045仿真次数图2.1|分量二状态分量二估计分量二预测II607080值数90100110120130仿真次数(3)绘出预测误差协方差阵迹(Trace)和估计误差协方差阵迹地曲线图;图3.14)对仿真结果进行分析.分量地估计值比分量地观测值更接近真实值.整个时也是估计值更准确.针对矢量情形，自行选取三组不同地参数进行Kalman

16、滤波地仿真，并进行相应仿真结果地比较分析.改变Q变大(Q=4)尬谟二書B誤马二書B翠#二書B-11ossi?Qi?scoc比ossi-olg藤阜100仿真次数改变R变小（R=4）125120115110105859095100105110115120125130仿真次数43210-1-3-2值数分量二状态分量二估计分量二预测11111111oh2030405060708090仿真次数仿真次数改变H变小（H=0.99）0尬谟二書B誤马二書B翠#二書B-81001-06080Z090901?藤阜oro力9917017S0比OSSI-0I-92-02-SI-0I-g-尬谟一書B誤马一書B翠#書B-1

17、-|当R地值变小时，预测值地阵迹会变得下坠更快，预测值本身地震荡会减小，对真实值地偏离会变小.当Q地值增大时，估计值也会更加偏离真实值.当H变小时，预测值与真实值偏差变大，估计值与真实值地偏差也会变大.二)基于线性Kalman滤波信息融合算法考虑如下一类多传感器线性动态估计系统TOC o 1-5 h zx(k)二0(k,k-l)x(k-1)+w(k,k-1)(10)z(k)=H(k)x(k)+v(k),i二1,2,N(11)iii其中，k0是离散地时间变量，N为传感器地数目；X(k-1)GRnxl是系统地状态向量，O(k,k-1)GRnxn是系统地状态转移矩阵；z(k)gRPi%1是状态x(k

18、)地观测向量，iH(k)GRpxn是相应地观测矩阵；w(k,k-1)和v(k)是零均值地高斯白噪声过程，且满i足如下条件：E和(k,k-1)=0.EW(k,k-1)w(j,j-1)t/=Q(k,k-1)J0(12)EQ(k)v(j)tl=R(k)5JiiiIk,jEW(k,k-1)v(j)t/=0i初始状态x(0)为一随机向量，且满足E*(0)=Xi(13)e1x(o)-xX(o)-x丨Lp000那么，对于每一个传感器观测z(k)均可执行一)当中基于单个观测地Kalman滤波估i计，可得到N个局部估计X(kIk)和相应地估计误差协方差矩阵P(kIk)(i二1,2,N).ii从而，可利用分布式加

19、权融合技术将上述N个局部Kalman滤波估计进行融合，即：x(kIk)=P(kIk)P-i(kIk)x(kIk)(14)iii=1P-1(kIk)=迓P-1(kIk)ii=1此时，X(kIk)和P(kIk)为融合后地状态估计和相应地融合估计误差协方差矩阵问题：给定相应参数(也鼓励采用其他参数)，进行上述分布式融合算法地仿真给定如下参数(i=1,2;N=2)：x(0)=1;0.1，X=10;1，P=100,10;10,10000(k,k-1)=1,1;0,1，Q(k,k-1)=0.1,0;0,0.1，H(k)=1,0;0,1，R(k)=0.1,0;0,0.1ii(1)请利用Matlab软件进行分

20、布式融合估计算法仿真程序编写；%mykf.m%producesystemclear。clc。A=11。01。P0=10010。10100。xO=1。0.1。X_0=10。1。C=10。01。Q=0.10。00.1。R=40。04。%guanceqi1realstatesandmeasurestatesfork=1:150W(:,k)=sqrt(Q)*randn(2,1)。V1(:,k)=sqrt(R)*randn(2,1)。ifk=1X1(:,k)=A*x0+W(:,k)。Z1(:,k)=C*X1(:,k)+V1(:,k)。elseX1(:,k)=A*X1(:,k-1)+W(:,k)。Z1(:

21、,k)=C*X1(:,k)+V1(:,k)。endend%predictstatesandestimatestatesfork=1:150ifk=1X_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_yc

22、1(:,:,k)。T_gj1(k)=trace(P_gj1(:,:,k)。elseX_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_g

23、j1(k)=trace(P_gj1(:,:,k)。endend%guanceqi2realstatesandmeasurestatesfork=1:150V2(:,k)=sqrt(R)*randn(2,1)。ifk=1X2(:,k)=A*x0+W(:,k)。Z2(:,k)=C*X2(:,k)+V2(:,k)。elseX2(:,k)=A*X2(:,k-1)+W(:,k)。Z2(:,k)=C*X2(:,k)+V2(:,k)。endend%predictstatesandestimatestatesfork=1:150ifk=1X_yc2(:,k)=A*X_0。Z_yc2(:,k)=C*X_yc2(

24、:,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(:,:,k)。elseX_yc2(:,k)=A*X_gj2(:,k-1)。Z_yc2(:,k)=C*X_yc2(:,k)。P_yc2(:,:,k)=A*P_g

25、j2(:,:,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(:,:,k)。endend%ronghefork=1:150P_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%createfigurefiguret=1:150。plot(t,X2(1,t),r,t,Z2(1,t),g)holdonplot(t,X_gj2(1,t),-ok,t,X_rhgj(1,t),-)holdofflegend(观测器2状态一,预测分量一,估