控制系统仿真课程设计

1、控制系统仿真课程设计一基于 Kalman滤波的信息融合算法设控制系统仿真课程设计（2011 级）题目控制系统仿真课程设计学院 专业班级 学号学生姓名指导教师完成日期2014年6月25日控制系统仿真课程设计一、题目基于Kalman滤波的信息融合算法设计1）学习并掌握线性系统Kalman滤波的基本原理和基本公式；2）学习并掌握一种常用的融合算法；3）学习并利用Matlab软件实现基本的Kalman滤波和信息融合算法的仿真。二、主要要求1）具备基本的概率与数理统计知识；2）熟悉并掌握基本的Matlab软件编写能力；3）学习并掌握正交投影定理和矩阵求逆定理；4）了解Kalman滤波的功能、来源和基本原

2、理；5）掌握Kalman滤波的推导过程和基本运行公式；6）了解信息融合的基本概念和方法；7）掌握一种典型的多传感器信息融合算法：分布式局部估计值加权融合。三、主要内容一）线性系统的Kalman滤波考虑如下一类单传感器线性动态估计系统x伙)=4伙,k -l)x 伙一 1) +w 伙,R -1)其中，Rno是离散的时间变量；X伙-l)e/?/,xl是系统的状态向量， 伙J-l)e /T是系统的状态转移矩阵；z伙)e尺网是状态x伙)的观测向量， H伙)e Rz是相应的观测矩阵；w伙1)和v伙)是零均值的高斯白噪声过程， 且满足如下条件：Ew(U-l) = 0E&(以-l)w(Q -1)= Q(k、k

3、f-Ev(Zr)=0, k、j0E&伙)v(;)7=R伙妙打Ew 伙,R-1)vO)=0初始状态x(0)为一随机向量，且满足(4)fx(O) = xoE&(0) - xox(O) -x0r= Po那么，线性系统的Kdlman滤波基本公式如下： 计算状态的一步预测值殳伙 M -1)=伙,R-)x(k-11-1) (5) 计算一步预测误差协方差阵P伙 M -1)=伙,k 一 1)P 伙-Wk-1)0 伙,k -1)7 + Q伙,k-l) (6) 计算增益阵K伙)= P(kk- 1)HT 伙)H 伙)Pi(k I k - 1)H7 伙)+ R 伙) 计算状态估计值x(kk) = x(kk-i) +

4、K伙)z(k)-H伙斥伙丨k一 1)和估计误差协方差阵P伙 I 幻=I 一 K(約H伙)P伙丨 一 1)(9)其中x(k-k-)和P伙-11 k -1)为 -1时刻的状态估计以及相应的估计误差协 方差阵。那么，Kalman滤波仿真程序执行方案如下：i) 确定初始状态x(0)、初始状态估计和和相应的协方差矩阵P；给定状态转移矩阵伙,-1)、过程噪声方差Q伙亲-1)、测量矩阵H伙)和测 量噪声方差R伙)(这些量均可认为是常量)ii) 产生仿真信号数据从ki:L开始循环(为给定的仿真时刻长度)a) 当k = 1时al)利用Q伙,k -1)和随机函数产生一个高斯白噪声w(l,O)；a2)根据式有x(l

5、) = 0(1,0)x(0)+ w(l,O)；a3)利用R伙)和随机函数产生一个高斯白噪声v(l)；a4)根据式有z(l) = H(l)x+ v(l)。b) 当k = 2丄时bl)利用Q伙,k-1)和随机函数产生一个高斯白噪声Wk、k I)；b2)根据式有x伙)=伙,k-l)x伙-1) + w(匕k-)；b3)利用R伙)和随机函数产生一个高斯白噪声v伙)；b4)根据式(2)有z伙)=H伙)x伙)+ v伙)。iii) 开始Kalman滤波估计从 = 1:厶开始循环(为给定的仿真时刻长度)a)当k = l时al)根据式(5)和式(6)有x(ll 0) = O(l,0)x0,P(11 0) = 0(

6、l,O)Po(l,O)y + Q(l,0)a2)利用式(7)-(9)计算估计/I ID和相应的估计误差协方差矩阵P(ll 1)。b)当k = 23、丄时bl)根据式和式计算xUIAr-1)和P伙1 1);b2)利用式(7)-(9)计算估计x(k I k)和相应的估计误差协方差矩阵p(m问题：给定相应参数(也鼓励采用其他参数)，进行Kalman滤波估计算法程 序的编写，并进行绘图和分析1)标量情形：x(0) = 1,心=10, Po = 100 ,伙1, Q伙我一 1) = 0.1, H伙)= 1,R伙)= 0(1) 请利用Matlab软件进行Kalman滤波估计仿真程序编写；%初始化clear

7、;A=l；P0=100;X0=10;X0_est=l;H=l;Q=0.1；R=8;%产生仿真信号和数据for k=l:100W (k) =sqrt (Q) *randn (1) ; %产生高斯白噪声WV(k)=sqrt(R)*randn；产生高斯白噪声Vif k=lX(k)=A*X0+W(k);Z(k)=H*X(k)+V(k);elseX(k)=A*X(k-l)+W(k); %状态值Z (k) =H*X (k) +V (k); %观测向量endend%Kalman 滤波for k=l:100辻 k=lX_yc (k) =A*X0_est;P_yc (k)=A*P0*A, +QelseX_yc

8、(k)=A*X (k-1) ;%状态预测值P_yc (k) =A*P_gj (k-1) *A +Q;% 协方差预测值endK (k) =P_yc (k) *H *inv (H*P_yc (k) *H +R); %增益矩阵心 j (k) =Oc (k) +K (k) * (Z (k) -H*Oc (k); %状态估计值P_gj (k) = (eye -K (k) *H) *P_yc (k); %协方差估计值end%绘制状态估计与状态预测值的曲线图figure(1)hold onPlot (X_gj, t);hold onplot (X_yc, -ob);hold onplot (X, ,-*g,

9、);legend (状态估计值，状态预测值，状态)title C状态估计与状态预测值的曲线图)xlabelf仿真次数)ylabel (数值)%绘制预测误差协方差和估计误差协方差曲线figure (2)hold onplot (P_gj, r)；hold onplot (P_yc, -ob);legend C估计误差协方差，预测误差协方)titleC估计误差协方差与预测误差协方的曲线图)xlabelf仿真次数)ylabelf 数值)(2) 绘出状态预测值和状态估计值的曲线图；12115II1051095985975(3) 绘出预测误差协方差和估计误差协方差的曲线图;(4) 对仿真结果进行分析。预

10、测值和估计值都能够在一定程度上反应真实值，但是估计值比观测值更 接近真实值。状态估计值：x(kk) = x(kk-) + K伙)z(灯-H(k)x伙I k-1)表明估计值是在预 测值的基础上进行优化后得到结果，所以估计值更准确一些。2)矢量情形：x(0) = l;0.1, x0=10;l, Po =100,10; 10,100,伙, =Q伙，氐一l) = 0丄0;0,0.1, H伙)=1,0;0,1, R伙)=0.1,0;0,0.1(1) 请利用Matlab软件进行Kalman滤波估计仿真程序编写；%程序初始值clear;A=l 1;0 1;P0=100 10;10 100;x0=l;0.1;

11、X_0=10;l;H=l 0;0 1;Q=0.1 0;0 0. 1;R=5 0;0 5：%产生仿真信号数据for k=l:50W (:, k) =sqrt (Q) *randn (2,1);V(:, k) =sqrt (R)*randn(2,1);辻 k=lX(:,k)=A*xO+W(:, k);Z(:,k)=H*X(:,k)+V(:,k);elseX(:,k)=A*X(:,k-l)+W(:, k);Z(:, k)=H*X(:,k)+V(:, k)；end end%Kalman 滤波for k=l:50辻 k=lX_yc (:, k)=A*X_0;P_yc(:, :,k) =A*P0*A, +

12、Q;T_yc(k) =trace(P_yc(:, :,k);elseX_yc(:, k)=A*X_gj (:, k-1);Z_yc (:, k) =H*X_yc (:, k);P_yc(:, :,k)=A*P_gj(:, :,+Q;T_yc(k) =trace(P_yc(:, :, k);endK(:, :, k)=P_yc(:, :, k)*H *inv(H*P_yc(:, :, k)*H +R);X_gj (:, k)=X_yc(:, k)+K(:, :, k)*(Z(:, k)-H*X_yc(:, k);P_gj (:, :, k) = (eye一K(:, :, k)*H)*P_yc(:

13、, :, k);T_gj (k) =trace (P_gj (:，:, k);end%绘制状态预测值和状态估计值分量一曲线 figure (1)k=l:50;Plot (k, X_gj (1, k),，-or )hold onplot (k, X_yc (1, k),J )hold onplot (k, X (1, k), -*b)hold offlegend C分量一估计，分倉一预测，分量一状态)xlabel (*仿真次数)ylabel (数值)%绘制状态预测值和状态估计值分量二曲线figure (2)Plot (k, X_gj (2, k), -ok)hold onplot (k, X_y

14、c(2, k),r)hold onplot (k, X (2, k), -*b)hold offlegend f分量二估计，分量二预测，分量二状态)xlabel C仿真次数)ylabel (数值)%绘制预测误差协方差阵迹和估计误差协方差迹的曲线figure(3)plot (k, T_gj (k), -ok, k, T_yc (k),1 r )legend C估计，预测)xlabel C仿真次数)ylabel (数值)(2) 绘出状态预测值和状态估计值的曲线图(每个状态包括两个分量);图11图2.1（3）绘出预测误差协方差阵迹血型和估计误差协方差阵迹的曲线图;图3.1(4) 对仿真结果进行分析。

15、分量的估计值比分量的观测值更接近真实值。整个时也是估计值更准确。 针对矢量情形，自行选取三组不同的参数进行Kalman滤波的仿真，并进行相应 仿真结果的比较分析。改变Q变大(Q=10)対JI夫败25031M(15r2SB分 t-分=2改变R增大（R=10）怙计12010】io：?3改变H变小(H=0. 3)当R的值变小时，预测值的阵迹会变得下坠更快，预测值本身的震荡会减 小，对真实值的偏离会变小。当Q的值增大时，估计值也会更加偏离真实值。 当H变小时，预测值与真实值偏差变大，估计值与真实值的偏差也会变大。 二)基于线性Kalman滤波信息融合算法考虑如下一类多传感器线性动态估计系统x伙)=4

16、伙,k-l)x 伙一 1) +w 伙,-1)(10)z,伙)=伙)x伙)+ 比伙)，Z = 1,2,N(11)其中，k0是离散的时间变量，N为传感器的数目；x伙-1) e 是系统的状 态向量，伙是系统的状态转移矩阵；是状态伙)的观 测向量，H伙)是相应的观测矩阵；w伙北-1)和匕伙)是零均值的高斯白 噪声过程，且满足如下条件：-fw伙,1) = 0E&伙,R - l)w(y, j-l)r= Q 伙,1)6(12)oev,伙)JU)7=R,()6jew(U_1)v0=O初始状态x(0)为一随机向量，且满足(13)；x(O)=xo Ex(O)-x()x(O)-x()r=P()那么,对于每一个传感器

17、观测z,a)均可执行一)当中基于单个观测的Kalman 滤波估计，可得到N个局部估计匕伙M)和相应的估计误差协方差矩阵 PIR)(f = l,2,2)。从而，可利用分布式加权融合技术将上述N个局部 Kalman滤波估计进行融合，即：Nx(k I灯=工P伙丨灯伙I k)xt伙I k)j N(14)pT伙伙)=工町伙|灯/-I此时，殳伙I k)和P伙I幻为融合后的状态估计和相应的融合估计误差协方差矩阵。问题：给定相应参数(也鼓励釆用其他参数)，进行上述分布式融合算法的 仿真给定如下参数(7 = 122 = 2)：x(0) = l;0.1, x0=10;l, Po =100,10; 10,100,伙

18、, =Q(k,E-1) = 0.1,0; 0,0.1, H伙) = l,0;0,1, R) = 0.1,0; 0,0.1(1) 请利用Matlab软件进行分布式融合估计算法仿真程序编写；%初始化clear;clc;A=l l;0 1;P0=100 10;10 100;x0=l;0.1;X_0=10;l;H=l 0;0 1;Q=0.1 0;0 0. 1;R=4 0;0 4：%观测器一真实数据for k=l:50W (:, k) =sqrt (Q) *randn (2,1);VI (：, k)=sqrt (R)*randn(2, 1);if k=lXl(：,k)=A*xO+W(:, k); Zl(

19、:,k)=H*Xl(:,k)+Vl(:,k);elseXl(:,k)=A*Xl(:,k-l)+W(:,k);Zl(:,k)=H*Xl(:,k)+Vl(:,k);endend%预测数据和估计数据1for k=l:50if k=lX_ycl(:,k)=A*X_0;Z_ycl(:,k)=H*X_ycl(:,k);P_yc 1(:, :,k) =A*P0*A, +Q;T_ycl (k) =trace (P_ycl (:，:, k);elseX_ycl (:, k)=A*X_gjl(:, k-1);Z_ycl(:,k) =H*X_ycl(:,k);P_ycl(:, :,k)=A*P_gjl(:, :,k

20、-D+A* +Q;T_ycl(k)=trace(P_ycl(:, :,k);endKl(：, :,k)=P_ycl(：, :, k)*HV(H*P_ycl(：, :,k)*H +R)；X_gjl(:,k)=X_ycl(:,k)+Kl(:, :,k)*(Zl(:,k)-Z_ycl(:,k);P_gjl(：, :, k) = (eye(2)K1(:, :, k)*H)*P_ycl(:, :, k);T_gj 1 (k) =trace(P_gj 1(:, :,k);end%观测器2真实数据for k=l:50V2(:, k)=sqrt (R)*randn(2, 1);if k=lX2(:,k)=A*

21、xO+W(:, k);Z2(:,k)=H*X2(:,k)+V2(:,k);elseX2(:,k) =A*X2(:, k-1) +W(:, k);Z2(:,k)=H*X2(:,k)+V2(:,k);endend%预测数据和估计数据2for k=l:50辻 k=lX_yc2(:,k)=A*X_0;Z_yc2(:,k) =H*X_yc2(:,k);P_yc2(:, :,k) =A*P0*A, +Q;T_yc2(k)=trace(P_yc2(:, :, k);elseX_yc2(:,k)=A*X_gj2(:,k-l);Z_yc2(:,k)=H*X_yc2(:,k);P_yc2(:, :,k)=A*P_

22、gj2(:, :,k-l)*A +Q;T_yc2(k)=trace(P_yc2(:, :,k);endK2(:, :, k)=P_yc2(:, :, k)*H/(H*P_yc2(:, :, k)*H +R)；X_gj2(:,k)=X_yc2(:,k)+K2(:, :,k)*(Z2(:,k)-Z_yc2(:,k);F_gj2(:, :,k) = (eyeK2(:, :,k)*H)*P_yc2(:, :,k);T_gj2 (k) =trace (P_gj2 (:，:, k);end%融合for k=l:50P_rh(:, :, k)=inv(P_gj2(:, :, k)+inv(P_gjl (:, :, k);Pjhgj(:, :,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_gjl(:,:,k)*X_gjl(:,k);end%绘制融合状态估计局部1局部2分量一曲线figure (1)t=l：50;plot (t, X_rhgj (1, t), -ok, t,