随机系统滤波与控制大作业.docVIP

PAGE PAGE 6 随机系统滤波与控制大作业 姓名：史贝贝 学号：3107040016 班级：硕736 专业：检测技术与自动化装置 已知一物体作自由落体运动，对其高度进行了20次测量，测量值如下表： 时间[s] 1 2 3 4 5 6 7 高度[km] 1.9945 1.9794 1.9554 1.9214 1.8777 1.8250 1.7598 时间[s] 8 9 10 11 12 13 14 高度[km] 1.6867 1.6036 1.5092 1.4076 1.2944 1.1724 1.0399 时间[s] 15 16 17 18 19 20 高度[km] 0.8980 0.7455 0.5850 0.4125 0.2318 0.0399 设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列，试求该物体高度和速度随时间变化的最优估计。（） 解： (1).理论分析 设时刻的位移、速度、加速度和加加速度分别为。对运动物体的跟踪者来说，是随机量，此处取为白噪声。 由运动学方程可得： 观测方程 其中。（题目中所 给高度数据单位为Km,应先将其化为单位m）. 取状态向量 则状态方程为 观测方程为 其中。 可见，这是随机线性定常系统的滤波问题。 应用Kalman基本滤波方程式，有 初始条件的选取： 中速度是运动学方程得出。如果系统是稳定的，那么初始条件的选取对结果的影响很小。从以后的实验结果可以看出。滤波方差最终稳定在某一数值上，系统是稳定的。 (2).计算结果与结果分析 1).计算得物体高度随时间变化的最优估计 可见用卡尔曼滤波法得到的高度滤波值在初始时与理想值有一定误差，在递推步数k增大时，误差值越来越小 2).计算得物体速度随时间变化的最优估计 可见用卡尔曼滤波法得到的速度滤波值在初始时与理想值有一定误差，在递推步数k增大时，误差值越来越小 3).高度协方差 可见用卡尔曼滤波法得到的高度协方差在初始时较大，为100，在递推步数k3时迅速减小，在k3时不超过2。 4).速度协方差 可见用卡尔曼滤波法得到的速度协方差在初始时较大为2，在递推步数k5时迅速减小，在k5时，不超过0.2，k10时接近于0。 (3).源程序 %卡尔曼滤波算法在估计自由落体轨迹中的应用 %输入观测值,此处对高度H做出观测 Z=[1994.5 1979.4 1955.4 1921.4 1877.7 1825.0 1759.8 1686.7 1603.6 1509.2 1407.6 1294.4 1172.4 1039.9 898.0 745.5 585.0 412.5 231.8 39.9]; %初始状态向量 X=[1993.5;-10;-9.8]; %初始方差矩阵 D=[10 0 0;0 10 0;0 0 1]; %观测间隔 T=1; %状态方程系数矩阵 fea=[1 T T^2/2; 0 1 T; 0 0 1]; tao=[0;0;T]; H=[1 0 0]; %噪声方差 r=1; %观测噪声 q=1; %过程噪声 %调用子函数KALMAN [tempX,varH,varV,varA]=KALMAN1(Z,X,D,fea,tao,H,r,q); %处理结果 n=length(Z); i=1:n; figure(1) plot(i,Z,i,tempX(1,:),r*-) title(对高度进行滤波) legend(滤波前,滤波后) figure(2) plot(i,tempX(2,:)) title(速度估计) figure(3) plot(i,tempX(3,:)) title(加速度估计) figure(4) plot(i,varH,i,varV,r+-,i,varA,-..) title(高度、速度、加速度方差估计) legend(高度,速度,加速度) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %子程序 %卡尔曼滤波算法 function [tempX,varH,varV,varA]=KALMAN1(Z,X,D,fea,tao,H,r,q) %子程序返回值：3个状态变量中间值，3个状态变量滤波方差变化 n=length(Z); %计算观测值长度 I=eye(3); %下面定义的变量用于存储计算过程滤波方差变化 varH=zeros(1,n); %高度方差 varV=zeros(1,n); %速度方差 varA=zeros(1,n); %加速度方差 %变量tempX用于存储状态变化 tempX=zeros(3,n); for j=1:n; tempX(:,j)=X; varH(1,j)=D(1,1); varV(1,j)=D(2,2);