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

1、 随机系统滤波与控制大作业姓名：史贝贝学号：3107040016班级：硕736专业：检测技术与自动化装置时间s1234567高度km1.99451.97941.95541.92141.87771.82501.7598时间s891011121314高度km1.68671.60361.50921.40761.29441.17241.0399时间s151617181920高度km0.89800.74550.58500.41250.23180.03991.已知一物体作自由落体运动，对其高度进行了20次测量，测量值如下表：设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列，试求该物体高度和速度随时

2、间变化的最优估计。(g=9.80m/s2)解：(1).理论分析设t时刻的位移、速度、加速度和加加速度分别为h,v,a,j。对运动物体的跟踪kkkkk者来说丿；是随机量此处取为白噪声。由运动学方程可得：aT2 HYPERLINK l bookmark2 o Current Document =h+vT+k-1k-12v=v+aTkk-1n-1a=a+jTkk-1k-1观测方程二h+Nkk其中T=1s,EN=0,DN,Nt=6,Ej=0,Dj,jT=8。（题目中所给高度kklklkklkl数据单位为Km,应先将其化为单位m）.（h）kvka丿k取状态向量xk二则状态方程为x二兀+口kk-1k-1观

3、测方程为z=h+N=Hx+Nkkkkkk厂1tT2/2、0、其中二01T,r=01JTvT丿可见，这是随机线性定常系统的滤波问题。应用Kalman基本滤波方程式，有,H=（100）。kx二x+KzHxkk1kkk1P二PT+FTtk,k1k1P二IKHPkkk,k1K二PHtkk1000、1443.5初始条件的选取：P二0100,x(0)二10、001,、9.8,x(0)中速度是运动学方程得出。如果系统是稳定的，那么初始条件的选取对结果的影响很小。从以后的实验结果可以看出。滤波方差最终稳定在某一数值上，系统是稳定的。(2).计算结果与结果分析1).计算得物体高度随时间变化的最优估计可见用卡尔曼

4、滤波法得到的高度滤波值在初始时与理想值有一定误差，在递推步数k增大时，误差值越来越小2).计算得物体速度随时间变化的最优估计可见用卡尔曼滤波法得到的速度滤波值在初始时与理想值有一定误差，在递推步数k增大时，误差值越来越小3).高度协方差可见用卡尔曼滤波法得到的高度协方差在初始时较大，为100,在递推步数kv3时迅速减小，在k3时不超过2。4).速度协方差可见用卡尔曼滤波法得到的速度协方差在初始时较大为2,在递推步数kv5时迅速减小,在k5时，不超过0.2，k10时接近于0。(3).源程序%卡尔曼滤波算法在估计自由落体轨迹中的应用%输入观测值，此处对高度H做出观测Z=1994.51979.419

5、55.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;0100;001;%观测间隔T=1;%状态方程系数矩阵fea=lTTA2/2;01T;001;tao=0;0;T;H=100;%噪声方差r=1;%观测噪声q=1;%过程噪声%调用子函数KALMANtempX，varH，varV，varA=KALMAN1(Z，X，D，fea，tao，H，r，q);%处理结果n=length

6、(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(高度,速度,加速度)%子程序%卡尔曼滤波算法functiontempX,varH,varV,varA=KALMAN1(Z,X,D,fea,tao,H,r,q)%子程

7、序返回值：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);tempD=fea*D*fea+q*tao*tao;K=tempD*H*inv(H*tempD*H+r)

8、;X=fea*X+K*(Z(1,j)-H*fea*X);D=(I-K*H)*tempD;end2.同样考虑自由落体运动的物体，用雷达（和物体落地点在同一水平面）进行测量，如图所示。如果雷达测距和测角的测量噪声是高斯白噪声随机序列，均值为零、方差阵0.040R=0001,试根据下列测量数据确定物体的高度和速度随时间变化的估计值。时间s*10000.000500000000000.001000000000000.001500000000000.002000000000000.002500000000000.003000000000000.003500000000000.00400000000000

9、0.004500000000000.005000000000000.005500000000000.006000000000000.006500000000000.007000000000000.007500000000000.008000000000000.008500000000000.009000000000000.009500000000000.010000000000000.010500000000000.011000000000000.011500000000000.012000000000000.012500000000000.013000000000000.0135000000

10、00000.014000000000000.014500000000000.015000000000000.015500000000000.016000000000000.016500000000000.017000000000000.017500000000000.018000000000000.018500000000000.01900000000000斜距km2.827416437818912.825198117297712.820666869662362.814872331059012.806717865362442.797252689740892.786642734750392.77

11、3203650263132.759195354645512.743312886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.590381098507852.562987942728432.534983179507972.506475893722462.475710750163862.445606760009822.414036907720882.382522286116962.350165011823322.31790939837137

12、2.285976166564532.254184316814012.222593202195352.192373989694662.162901779972712.134417257937062.108110646907272.083221798231952.061481090267672.042198850940312.026102353143572.01290326863579俯仰角rad*10000.000758504358760.000832822604780.000678082416390.000852790368020.000729007684520.000800724818190

13、.000750955762130.000657627253790.000811861485450.000797837270340.000730607129860.000632420065300.000636565244950.000806598456390.000677047400690.000765737677060.000549557590810.000584879139710.000556027473680.000335504125880.000560126884520.000566944919780.000593806310250.000536819165440.00065871960

14、7810.000685983443280.000609224713480.000570860189180.000413085357080.000473020262810.000309493099720.000405526249860.000375450331420.000172823192620.000207583279800.000371864645790.000180821634650.000233238301600.019500000000002.00463157388395-0.00004536186964题2示意图dv(t)二dv(t)二gdh(t)=v(t)dtdt000r0即:加

15、dt001X(t)+0000_g_0001解：(1).理论分析d应该选取高度h(t)，速度v(t),d为状态变量x(t)，即:X(t)二h(t)v(t)根据自由落体系统的机理，选取向上为正方向，则有:根据连续时间描述离散化的相关知识，F=00-10000C=0。则有:_g_)TOC o 1-5 h z100Q(t,t)=I+F(tt)=01(tt001100故：0(k+1,k)=01T001屮(k+1,k)=”r(k+1),tc(t)dT二故系统的状态方程为：100_0_X(k+1)二01TX(k)+0.5gt2001_gt_kT又因为给定的量测值是斜距，俯仰角，故系统的量测方程为：一0.5g

16、t2gtY(k+1)二L(k+1)9(k+1)、h2(k+1)+d2h(k+1)arctand+V(k+1)辅助方程：H(k+1)XX(；+n+1|dh(k+1Ik)2：h(k+1Ik)+d2；a2h(k+1Ik)+d2-h(k+1Ik)dA2h(k+1Ik)+d2A2h(k+1Ik)+d2A00AX(k+1)=X(k+1lk)则系统量测方程变为：Y(k+1)二H(k+1)X(k+1)+V(k+1)0.04题目要求给出该物体高度和速度随时间变化的的最优估计值，并且给定了斜距，俯仰角测量值，故可以用卡尔曼滤波解决问题。0.0419955A滤波初值：X(0I0)二2005P(0Ig512T=0.5

17、R(k)二Q(k)=OY(k)二0.01AA一步预测：f(k+11k)=Q(k+1,k)f(kIk)+U(k)P(k+1Ik)=e(k+1,k)P(kIk)Qt(k+1,k)滤波增益：K(k+1)二P(k+1Ik)Ht(k+1)H(k+1)P(k+1Ik)Ht(k+1)+R(k+1)-iAAA滤波计算：X(k+1Ik+1)=X(k+1Ik)+K(k+1)Y(k+1)h(X(k+1Ik),k+1)P(k+1Ik+1)二I一K(k+1)H(k+1)P(k+1Ik)(2).计算结果与结果分析1).高度滤波值2).速度滤波值可见用扩展卡尔曼滤波法得到的速度滤波值与理想值在初始几步有一些误差，再k5时误

18、差很小。3).高度协方差4).速度协方差用扩展卡尔曼滤波法得到的速度协方差初始值为2,在kv5时以指数形式迅速减小，在k5时不超过0.1。(3).源程序%卡尔曼滤波算法在估计自由落体轨迹中的应用%直接测量值为斜距和俯仰角，要求对高度H和速度V做出预测formatlong;%为斜距，t为俯仰角r=2.827416437818912.825198117297712.820666869662362.814872331059012.806717865362442.797252689740892.786642734750392.773203650263132.759195354645512.743312

19、886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.590381098507852.562987942728432.534983179507972.506475893722462.475710750163862.445606760009822.414036907720882.382522286116962.350165011823322.317909398371372.285976166564532.254184316814012.22

20、2593202195352.192373989694662.162901779972712.134417257937062.108110646907272.083221798231952.061481090267672.042198850940312.026102353143572.012903268635792.004631573883952.00058143251913*1000;t=0.000758504358760.000832822604780.000678082416390.000852790368020.000729007684520.000800724818190.000750

21、955762130.000657627253790.000811861485450.000797837270340.000730607129860.000632420065300.000636565244950.000806598456390.000677047400690.000765737677060.000549557590810.000584879139710.000556027473680.000335504125880.000560126884520.000566944919780.000593806310250.000536819165440.000658719607810.00

22、0685983443280.000609224713480.000570860189180.000413085357080.000473020262810.000309493099720.000405526249860.000375450331420.000172823192620.000207583279800.000371864645790.000180821634650.000233238301600.000045361869640.00003246284068*1000;%人为高度，dO为水平距离；两者作为观测值使用n=length(r);h=;dO=;fori=1:nh(i)=r(i)*sin(t(i);dO(i)=r(i)*cos(t(i);endZ=dO;h;%初始状态向量X=2O16.O;2OOO.O;-1;%初始方差矩阵D=5OO;O5O;OO1O;%观测间隔T=O.5;%状态方程系数矩阵fea=1OO;O1T;OO1;H=1OO;O1O;%调用子函数KALMANtempX,vardO,varH,varV=KALMAN1(Z,r,t,X,D,fea,H);%处理结果n=length(r);i=1:n;figure(1)plot(i,tempX(1,:)title(水平