资源描述：
《随机系统滤波与控制大作业》由会员上传分享，免费在线阅读，更多相关内容在学术论文-天天文库。
1、随机系统滤波与控制大作业姓名：史贝贝学号：3107040016班级：硕736专业：检测技术与自动化装置131.已知一物体作自由落体运动，对其高度进行了20次测量，测量值如下表：时间[s]1234567高度[km]1.99451.97941.95541.92141.87771.82501.7598时间[s]891011121314高度[km]1.68671.60361.50921.40761.29441.17241.0399时间[s]151617181920高度[km]0.89800.74550.58500.41250.2
2、3180.0399设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列，试求该物体高度和速度随时间变化的最优估计。（）解：(1).理论分析设时刻的位移、速度、加速度和加加速度分别为。对运动物体的跟踪者来说，是随机量，此处取为白噪声。由运动学方程可得：观测方程其中。（题目中所给高度数据单位为Km,应先将其化为单位m）.取状态向量则状态方程为观测方程为其中。可见，这是随机线性定常系统的滤波问题。应用Kalman基本滤波方程式，有13初始条件的选取：中速度是运动学方程得出。如果系统是稳定的，那么初始条件的选取对结果的影响很
3、小。从以后的实验结果可以看出。滤波方差最终稳定在某一数值上，系统是稳定的。(2).计算结果与结果分析1).计算得物体高度随时间变化的最优估计可见用卡尔曼滤波法得到的高度滤波值在初始时与理想值有一定误差，在递推步数k增大时，误差值越来越小2).计算得物体速度随时间变化的最优估计13可见用卡尔曼滤波法得到的速度滤波值在初始时与理想值有一定误差，在递推步数k增大时，误差值越来越小3).高度协方差可见用卡尔曼滤波法得到的高度协方差在初始时较大，为100，在递推步数k<3时迅速减小，在k>3时不超过2。4).速度协方差13可见用卡
4、尔曼滤波法得到的速度协方差在初始时较大为2，在递推步数k<5时迅速减小，在k>5时，不超过0.2，k>10时接近于0。(3).源程序%卡尔曼滤波算法在估计自由落体轨迹中的应用%输入观测值,此处对高度H做出观测Z=[1994.51979.41955.41921.41877.71825.01759.81686.71603.61509.21407.61294.41172.41039.9898.0745.5585.0412.5231.839.9];%初始状态向量X=[1993.5;-10;-9.8];%初始方差矩阵D=[1000
5、;0100;001];%观测间隔T=1;%状态方程系数矩阵fea=[1TT^2/2;01T;001];tao=[0;0;T];H=[100];%噪声方差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('滤波前','滤波后')13figure(2
6、)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,ta
7、o,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);forj=1:n;tempX(:,j)=X;varH(1,j)=D(1,1);varV(1,j)=D(2,2);varA(1,j)=D(3,3);temp
8、D=fea*D*fea'+q*tao*tao';K=tempD*H'*inv(H*tempD*H'+r);X=fea*X+K*(Z(1,j)-H*fea*X);D=(I-K*H)*tempD;end131.同样考虑自由落体运动的物体，用雷达（和物体落地点在同一水平面）进行测量，如图所示。如果雷达测距和测角的测量噪声是
