基于Kalman滤波的信息融合算法设计_控制系统仿真课程设计.docx

控制系统仿真课程设计题目控制系统仿真课程设计学院专业班级学号学生姓名指导教师完成日期控制系统仿真课程设计一、题目基于Kalman滤波的信息融合算法设计1) 学习并掌握线性系统Kalman滤波的基本原理和基本公式；2) 学习并掌握一种常用的融合算法；3) 学习并利用Matlab软件实现基本的Kalman滤波和信息融合算法的仿真。二、主要要求1) 具备基本的概率与数理统计知识；2) 熟悉并掌握基本的Matlab软件编写能力；3) 学习并掌握正交投影定理和矩阵求逆定理；4) 了解Kalman滤波的功能、来源和基本原理；5) 掌握Kalman滤波的推导过程和基本运行公式；6) 了解信息融合的基本概念和方法；7) 掌握一种典型的多传感器信息融合算法：分布式局部估计值加权融合。三、主要内容一）线性系统的Kalman滤波考虑如下一类单传感器线性动态估计系统 (1) (2)其中，是离散的时间变量；是系统的状态向量，是系统的状态转移矩阵；是状态的观测向量，是相应的观测矩阵；和是零均值的高斯白噪声过程，且满足如下条件：， (3)初始状态为一随机向量，且满足 (4)那么，线性系统的Kalman滤波基本公式如下： 计算状态的一步预测值(5) 计算一步预测误差协方差阵(6) 计算增益阵 (7) 计算状态估计值 (8)和估计误差协方差阵 (9)其中和为时刻的状态估计以及相应的估计误差协方差阵。那么，Kalman滤波仿真程序执行方案如下：i) 确定初始状态、初始状态估计和相应的协方差矩阵；给定状态转移矩阵、过程噪声方差、测量矩阵和测量噪声方差（这些量均可认为是常量）ii) 产生仿真信号数据从开始循环（L为给定的仿真时刻长度）a) 当时a1) 利用和随机函数产生一个高斯白噪声；a2) 根据式(1)有；a3) 利用和随机函数产生一个高斯白噪声；a4) 根据式(2)有。b) 当时b1) 利用和随机函数产生一个高斯白噪声；b2) 根据式(1)有；b3) 利用和随机函数产生一个高斯白噪声；b4) 根据式(2)有。iii) 开始Kalman滤波估计从开始循环（L为给定的仿真时刻长度）a) 当时a1) 根据式(5)和式(6)有, a2) 利用式(7)-(9)计算估计和相应的估计误差协方差矩阵。b) 当时b1) 根据式(5)和式(6)计算和；b2) 利用式(7)-(9)计算估计和相应的估计误差协方差矩阵。问题：给定相应参数（也鼓励采用其他参数），进行Kalman滤波估计算法程序的编写，并进行绘图和分析1) 标量情形：，（1）请利用Matlab软件进行Kalman滤波估计仿真程序编写；%mykf.m%produce systemclear;A=1;P0=100;X0=10;C=1;Q=0.1;R=10;%real states and measure statesfor k=1:150W(k)=sqrt(Q)*randn(1,1);V(k)=sqrt(R)*randn(1,1);if k=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%predict states and estimate statesfor k=1:150if k=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%create figurefiguret=1:150;plot(t,Z(1,t),-og)hold onplot(t,X_gj(1,t),r)hold onplot(t,X(1,t),b)hold offlegend(观测,估计,状态)xlabel(仿真次数)ylabel(数值)figureplot(abs(Z-X),-og);hold onplot(abs(X_gj-X);hold offlegend(预测与真实之差,估计与真实之差)xlabel(仿真次数)ylabel(数值)（2）绘出状态预测值和状态估计值的曲线图；（3）绘出预测误差协方差和估计误差协方差的曲线图；（4）对仿真结果进行分析。预测值和估计值都能够在一定程度上反应真实值，但是估计值比观测值更接近真实值。状态估计值：表明估计值是在预测值的基础上进行优化后得到结果，所以估计值更准确一些。2) 矢量情形：，（1）请利用Matlab软件进行Kalman滤波估计仿真程序编写；%mykf.m%produce systemclear;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=10 0;0 10;%real states and measure statesfor k=1:150 W(:,k)=sqrt(Q)*randn(2,1); V(:,k)=sqrt(R)*randn(2,1); if k=1 X(:,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%predict states and estimate statesfor k=1:150 if k=1 X_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); else X_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%create figurefigure t=1:150;plot(t,X(1,t),-or)hold onplot(t,X_gj(1,t),g)plot(t,Z(1,t),-)hold offlegend(分量一状态,分量一估计,分量一预测)xlabel(仿真次数)ylabel(数值)figure plot(t,X(2,t),-or,t,X_gj(2,t),g)hold onplot(t,Z(2,t),-)hold offlegend(分量二状态,分量二估计,分量二预测)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(数值)（2）绘出状态预测值和状态估计值的曲线图（每个状态包括两个分量）；图1.1图2.1（3）绘出预测误差协方差阵迹(Trace)和估计误差协方差阵迹的曲线图；图3.1（4）对仿真结果进行分析。分量的估计值比分量的观测值更接近真实值。整个时也是估计值更准确。针对矢量情形，自行选取三组不同的参数进行Kalman滤波的仿真，并进行相应仿真结果的比较分析。改变Q变大（Q=4）改变R变小（R=4）改变H变小（H=0.99）当R的值变小时，预测值的阵迹会变得下坠更快，预测值本身的震荡会减小，对真实值的偏离会变小。当Q的值增大时，估计值也会更加偏离真实值。当H变小时，预测值与真实值偏差变大，估计值与真实值的偏差也会变大。二）基于线性Kalman滤波信息融合算法考虑如下一类多传感器线性动态估计系统 (10)， (11)其中，是离散的时间变量，为传感器的数目；是系统的状态向量，是系统的状态转移矩阵；是状态的观测向量，是相应的观测矩阵；和是零均值的高斯白噪声过程，且满足如下条件：， (12)初始状态为一随机向量，且满足 (13)那么，对于每一个传感器观测均可执行一)当中基于单个观测的Kalman滤波估计，可得到个局部估计和相应的估计误差协方差矩阵。从而，可利用分布式加权融合技术将上述个局部Kalman滤波估计进行融合，即： (14)此时，和为融合后的状态估计和相应的融合估计误差协方差矩阵。问题：给定相应参数（也鼓励采用其他参数），进行上述分布式融合算法的仿真给定如下参数：，（1）请利用Matlab软件进行分布式融合估计算法仿真程序编写；%mykf.m%produce systemclear;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 real states and measure statesfor 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); else X1(:,k)=A*X1(:,k-1)+W(:,k); Z1(:,k)=C*X1(:,k)+V1(:,k); endend%predict states and estimate statesfor 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(:,:,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(:,:,k); endend%guan ce qi 2 real states and measure statesfor 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); endend%predict states and estimate statesfor k=1:150 if k=1 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(:,:,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(:,:,k); endend% 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 figurefiguret=1:150;plot(t,X2(1,t),r,t,Z2(1,t),g)hold onplot(t,X_gj2(1,t),-ok,t,X_rhgj(1,t),-)hold offlegend( 观测器2状态一,预测分量一, 估计分量一), 融合分量一)xlabel(仿真次数)ylabel(数值)figure