控制系统仿真课程设计

控制系统仿真课程设计（2010级）题目控制系统仿真课程设计学院专业班级学号学生姓名指导教师完成日期控制系统仿真课程设计、题目基于Kalman滤波地信息融合算法设计学习并掌握线性系统Kalman滤波地基本原理和基本公式；学习并掌握一种常用地融合算法；学习并利用Matlab软件实现基本地Kalman滤波和信息融合算法地仿真.二、主要要求具备基本地概率与数理统计知识；熟悉并掌握基本地Matlab软件编写能力；学习并掌握正交投影定理和矩阵求逆定理；了解Kalman滤波地功能、来源和基本原理；掌握Kalman滤波地推导过程和基本运行公式；了解信息融合地基本概念和方法；掌握一种典型地多传感器信息融合算法：分布式局部估计值加权融合三、主要内容一)线性系统地Kalman滤波考虑如下一类单传感器线性动态估计系统TOC\o"1-5"\h\zx(k)=鱼(k,k-1)x(k-1)+w(k,k-1)(1)z(k)=H(k)x(k)+v(k)(2)其中，k>0是离散地时间变量；x(k-1)GRnx1是系统地状态向量，①(k,k-1)GRnxn是系统地状态转移矩阵；Z(k)GRpx1是状态x(k)地观测向量，H(k)GRp邓是相应地观测矩阵；w(k,k-1)和v(k)是零均值地高斯白噪声过程，且满足如下条件：'E^w(k,k-1)}=0IE如k,k-1)w(j,j-5LQ(k,k-1)5<Ejv(k)}=0"，k,j>0(3)E\(k)v(j)J=R(f)5kj、EW(k,k-1)v(j)tJ=00J初始状态x(0)为一随机向量，且满足

‘E§(0)}=‘E§(0)}=x

E1x(0)-x0]X(0)-x}}=P计算状态地一步预测值X(kIk-1)=鱼(k,k-1)x(k-11k-1)(5)计算一步预测误差协方差阵P(kIk-1)二玺(k,k-1)P(k-1Ik-1)①(k,k-1)t+Q(k,k-1)⑹计算增益阵TOC\o"1-5"\h\zK(k)=P(kIk-1)Ht(k)H(k)P(kIk-1)Ht(k)+R(k)--1(7)计算状态估计值x(kIk)=X(kIk-1)+K(k)lz(k)-H(k)x(kIk-1)](8)和估计误差协方差阵P(kIk)=ll-K(k)H(k)^P(kIk-1)(9)其中X(k-1Ik-1)和P(k-1Ik-1)为k-1时刻地状态估计以及相应地估计误差协方差阵.那么，Kalman滤波仿真程序执行方案如下：确定初始状态x(0)、初始状态估计X和相应地协方差矩阵P；给定状态转移矩阵00也(k,k-1)、过程噪声方差Q(k,k-1)、测量矩阵H(k)和测量噪声方差R(k)(这些量均可认为是常量)产生仿真信号数据从k=1:L开始循环(L为给定地仿真时刻长度)当k=1时al)利用Q(k,k-1)和随机函数产生一个高斯白噪声w(1,0)；a2)根据式(1)有x(1)=Q(1,0)x(0)+w(1,0)；a3)利用R(k)和随机函数产生一个高斯白噪声v(1)；a4)根据式⑵有z(1)=H(1)x(1)+v⑴.当k=2,3,,L时bl)利用Q(k,k-1)和随机函数产生一个高斯白噪声w(k,k-1)；b2)根据式(1)有x(k)=①(k,k-1)x(k-1)+w(k,k-1)；b3)利用R(k)和随机函数产生一个高斯白噪声v(k)；b4)根据式⑵有z(k)=H(k)x(k)+v(k).开始Kalman滤波估计从k=1:L开始循环（L为给定地仿真时刻长度）a）当k=1时al)根据式(5)和式(6)有x(110)=①(1,0)x,P(110)=①(1,0)PQ(1,0)t+Q(1,0)

00a2)利用式(7)-(9)计算估计X(111)和相应地估计误差协方差矩阵P(111).b)当k=2,3,,L时bl)根据式(5)和式(6)计算X(kIk-1)和P(kIk-1)；b2)利用式(7)-(9)计算估计X(kIk)和相应地估计误差协方差矩阵P(kIk).问题：给定相应参数(也鼓励采用其他参数)，进行Kalman滤波估计算法程序地编写，并进行绘图和分析1)标量情形：x(0)=1，X0=10，P0=100，①(k,k-1)=1，Q(k,k-1)=0.1，H(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。%realstatesandmeasurestatesfork=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)*(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')holdonholdofflegend（'观测','估计','状态'）xlabel（'仿真次数'）ylabel（'数值'）figureplot（abs（Z-X）,'-og'）。holdonplot（abs（X_gj-X））。holdofflegend（'预测与真实之差'，'估计与真实之差'）xlabel（'仿真次数'）ylabel（'数值'）(2)绘出状态预测值和状态估计值地曲线图;4045505560657075808590仿真次数(3)绘出预测误差协方差和估计误差协方差地曲线图；4.53.5值3数2.521.51~~预测与真实之差危估计与真实之差30405060708090仿真次数(4)对仿真结果进行分析.预测值和估计值都能够在一定程度上反应真实值，但是估计值比观测值更接近真实值状态估计值：X(kIk)=X(kIk-1)+K(k)lz(k)-H(k)x(kIk-1)]表明估计值是在预测值地基础上进行优化后得到结果，所以估计值更准确一些.2)矢量情形：x(0)=[1;0.1]，xo=[10;1]，P0=[100,10;10,100]，中(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]。x0=[1。0.1]。X_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(:,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)*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),'--')holdofflegend（'分量一状态','分量一估计','分量一预测'）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_yc(t),'-or')legend('估计','预测')xlabel('仿真次数')ylabel('数值')绘出状态预测值和状态估计值地曲线图(每个状态包括两个分量)；图1.1分量一状态分量一估计分量一预测151050-5-10-15-20-25分量一状态分量一估计分量一预测151050-5-10-15-20-2510分量二状态分量二估计分量二预测1101201305101520253010分量二状态分量二估计分量二预测110120130仿真次数图2.160708090100仿真次数

绘出预测误差协方差阵迹(Trace)和估计误差协方差阵迹地曲线图;图3.112010080值数60402010205060估计」预测仿真0次数40对仿真结果进行分析.12010080值数60402010205060估计」预测分量地估计值比分量地观测值更接近真实值.整个时也是估计值更准确.针对矢量情形，自行选取三组不同地参数进行Kalman滤波地仿真，并进行相应仿真结果地比较分析.改变Q变大(Q=4)醇Y（草鲫099*017能0£9COS9L0LS赊瓯二喜好一K判二喜好翠姓二喜好-11醇Y（草鲫仿真次数改变R变小（R=4）125120115110105100859095100105110115120125130仿真次数估计

预测43210-1-2-32030405060708090估计

预测仿真次数40353025值数201510581012141618仿真次数改变H变小（H=0.99）醇Y（草鲫001-06080Z09090*9-*-赊瓯二喜好——:O■K判二喜好十||■翠姓二喜好——j]]1]]10L藤醇Y（草鲫9*017S£0£9COS9L赊瓯一喜好一K判一喜好翠姓一喜好-1-|0L9C-02-g-o当R地值变小时，预测值地阵迹会变得下坠更快，预测值本身地震荡会减小，对真实值地偏离会变小.当Q地值增大时，估计值也会更加偏离真实值.当H变小时，预测值与真实值偏差变大，估计值与真实值地偏差也会变大.)基于线性Kalman滤波信息融合算法考虑如下一类多传感器线性动态估计系统x(k)=鱼(k,k-1)x(k-1)+w(k,k-1)(10)z.(k)=H(k)x(k)+v.(k)，i=1,2,,N(11)其中，k>0是离散地时间变量，N为传感器地数目；X(k-1)GRnx1是系统地状态向量，G(k,k-1)GRnxn是系统地状态转移矩阵；z.(k)GRp’xl是状态x(k)地观测向量，H(k)GRpxn是相应地观测矩阵；w(k,k-1)和v(k)是零均值地高斯白噪声过程，且满i足如下条件：‘勺&(k,k-1)}=0]EW(k,k-1)w(j,j-1)r/=Q(k,k-1)5<砂.(k)}=0，k,j〉0(12)E\(k)v(j)t/=R(k)5i.k,jEW(k,k-1)v(j)t/=0i初始状态x(0)为一随机向量，且满足

‘E‘E§(0)}=x

E1x(0)-x°]X(0)-xIXp那么，对于每一个传感器观测zj(k)均可执行一)当中基于单个观测地Kalman滤波估计，可得到N个局部估计x,(kIk)和相应地估计误差协方差矩阵P(kIk)(i=1,2,,N).从而，可利用分布式加权融合技术将上述N个局部Kalman滤波估计进行融合，即：x(kIk)=£P(kIk)P-1(kIk)x.(kIk)(14)i=1(14)p-i(kIk)=1Lp-i(kIk)ii=1此时，x(kIk)和P(kIk)为融合后地状态估计和相应地融合估计误差协方差矩阵.问题：给定相应参数(也鼓励采用其他参数)，进行上述分布式融合算法地仿真给定如下参数(i=1,2；N=2):x(0)=[1;0.1]，x0=[10;1]，P0=[100,10;10,100]①(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]请利用Matlab软件进行分布式融合估计算法仿真程序编写；%mykf.m%producesystemclear。clcoA=[11o01]oP0=[10010o10100]oX0=[1o0.1]oX_0=[10o1]oC=[10o01]oQ=[0.10o00.1]oR=[40o04]o%guanceqi1realstatesandmeasurestatesfork=1:150W(:,k)=sqrt(Q)*randn(2,1)oV1(:,k)=sqrt(R)*randn(2,1)oifk==1X1(:,k)=A*x0+W(:,k)oZ1(:,k)=C*X1(:,k)+V1(:,k)oelseX1(:,k)=A*X1(:,k-1)+W(:,k)。Z1(:,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_yc1(:,:,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_gj1(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(:,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_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(:,:,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比Z2(1,t),'g')holdonplot(t,X_gj2(1,t),'-ok',t,X_rhgj(1,t),'--')holdofflegend('观测器2状态一二预测分量一','估计分量一),','融合分量一')xlab