已知一物体作自由落体运动对其高度进行了20次测量

1、1、已知一物体作自由落体运动，对其高度进行了20次测量，测量值如下表：时间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.0399Eh1900mh1000_和速度V也是高斯分布的随机变量，且0=,P=var0=0EV010m/s0V002设高度的测量误差是均值为0、方差为1的高斯白噪声随机序列，该物体的初始高度h试

2、求该物体高度和速度随时间变化的最优估计。（g二9.80m/s2）解：hk为物体在k时刻的高度，I为物体在k时刻的选取系统的状态变量为lhv1,kk瞬时速度。这里不考虑过程噪声的存在。由牛顿第二运动定律可以写出系统的状态方程：（由采样周T=ls,根据离散化的方法得离散的状态方程）X(k+1)=其中(k+1,k)二hk+i=v-k+i1-01-11hk+vk-0.51屮(k+1,k)二-0.51Y(k+1)=1hk+11+Vvk+1k+1建立如下的测量方程其中H（k+1）=101o应用卡尔曼滤波算法可以得到物体的高度和速度随时间变化的最优估计，卡尔曼滤波算法为：一步预测：x(k+1|k)=O(k+

3、1,k)X(k|k)+屮(k+1,k)U(k)预测方差：P(k+11k)=(k+1,k)P(k|k)0T(k+1,k)滤波增益：K(k+1)=P(k+1k)Ht(k+1)H(k+1)P(k+1|k)Ht(k+1)+R(k+1)-1滤波计算：PX(k+11k+1)=X(k+1|k)+K(k+1)Y(k+1)-H(k+1)X(k+1|k)滤波方差：P(k+1k+1)=1K(k+1)H(k+1)p(k+1k)由题设，滤波初始值为：A一Eh-1900m_h_1000_X(0)=0EV010m/sP=Var00V002测量误差为：EV(k+1)=0,VarV(k+1)=1A=1-1;01;B=-1/2;

4、1;C=10;U=9.80;R=1;h1=1994.51979.41955.41921.41877.71825.01759.81686.71603.61509.21407.6%一步预测%一步预测误差方差矩阵%滤波增益%滤波值%保存高度滤波值%保存速度滤波值%滤波方差1294.41172.41039.9898745.5585412.5231.839.9;x=190010;p=1000;02;t=1:20;he=zeros(1,length(t);fori=1:20 x=A*x+B*U;p=A*p*A;k=p*C*inv(R+C*p*C);x=x+k*(h1(i)-C*x);h2(i)=x(1,:

5、);v(i)=x(2,:);p=(eye(2)-k*C)*p;P1(i)=p(1,1)P2(i)=p(2,2)endfigure(1),plot(t,h1,r,t,h2,*);legend(滤波曲线，测量曲线)title(滤波曲线和测量曲线)figure(2)，plot(t，v);legend(速度曲线)title(速度曲线)figure(3)plot(P1，r);legend(高度方差)title(高度方差)figure(4)plot(P2，r)legend(速度方差)title(速度方差)运行程序，得到的物体高度和速度随时间变化的最优估计如下表所示：时间S测量高度m高度的最优估计m速度的最

6、优估计m/s11994.51993.417.67621979.41977.323.23831955.41953.730.5541921.41920.339.551877.7187748.96461825.01824.158.571759.81760.368.39181686.71686.978.21491603.61603.788.024101509.21510.297.909111407.61407.5107.7121294.41294.7117.52131172.41172.3127.32141039.91040.1137.1215898.0898.03146.9216745.5746.0

7、5156.7417585.0584.54166.5318412.5412.98176.3419231.8231.76186.142039.940.566195.95上表为物体随时间变化的高度量测值和高度，速度的最优估计值按此计算结果绘制的曲线图如下:2CC01EC01EC014C013201CC0ECO0:0JCOzr26610121J161620结果分析：人此题中我们利用初值X(0)和P(O)根据卡尔曼滤波算法递推的求出了物体的高度和速度的最优估计，从物体的高度的最优估计曲线中我们可以看到，物体的高度最优估计值和实际测量值几乎相等，物体的速度的最优估计也和自由落体的速度几乎一致，只是在初始滤

8、波时有一定的的偏差，这是由于滤波初值选取决定的。在许多实际问题中，往往不能精确知道初值。很显然此时计算得到的滤波值将不是最优的。根据相应的判定准则，我们知道该系统是一致完全能和一直完全能观的。即它的最优线性最优滤波系统是一直渐近稳定的，所以滤波初值选取只影响滤波的初始阶段，系统的滤波最终会趋向稳定。这是有系统自身的特性决定的。因此初值任意选取并不会影响滤波的最后阶段的结果。显然，上述结果是在测量误差矩阵R精确可知的情况下得到的。如果R不精确，那么由上述算法给出的X(k)将不是X(k)的最优估计。并且此时，滤波还可能发散。这时我们可以采用噪声不精确（未知）系统的自适应滤波：可以先选取一个适当的量

9、测噪声矩阵并将它固定下来，然后按照动态噪音不精确的情况设计自适应滤波。本题中假设测量误差为均值为0,方差为I的高斯白色噪声随机序列，并且初始高度和速度相互独立。显然这种假设是比较理想的状况。如果测量误差为有色噪声，此时我们可以通过适当的变换把系统方程中的有色噪声转变成白色噪声的情形处理，从而得到一个白色噪声作用下的离散线性系统，有此系统的滤波算法导出原系统的滤波算法。由上图可以看出，速度的滤波方差在最初的时间内很大（接近于2），高度滤波方差在初始时更大（接近于100）,但是很快速度滤波方差就收敛到0；而高度的滤波方差收敛速度比速度滤波方差的要慢，但是最终也收敛到了某个稳态值。改变参数看影响：将

10、滤波初值、初值方差、测量噪声方差分别变化，其相应滤波方差如图所示：初值方羞增加时高皮方雀和速皮方雀机值肓差威歩时高度方差和谨度方差测童唏声增加时高度方差柑迪度方差综上所示，滤波初值的改变对滤波方差影响不大（其实这一点在卡尔曼滤波算法中也可看出），而改变初值方差、测量噪声方差对滤波方差有较大的影响。初值方差和测量噪声方差的增加都会使高度方差和速度方差变大，且收敛速度变慢；相应地，他们的减少也都会使高度方差和速度方差变小，收敛速度变快。从统计学的角度讲，方差代表数据的精度，方差大则数据偏离真值副度就大，数据本身的可靠性就差，数据所带来的信息就小，所带来的误差就大（根据误差理论的误差可传递性）。，试

11、根据下列测量数据确0.012、同样考虑自由落体运动的物体，用雷达（和物体落地点在同一水平面）进行测量，如图一Ed一1995md一500_00Eh=2005m，P=varh=050000EV1-01m/sVL0J002，且雷达测距和测角的测量噪所示。如果0.04声是高斯白噪声随机序列，均值为零、方差阵R定物体的高度和速度随时间变化的估计值。时间s*10000.000500000000000.001000000000000.001500000000000.002000000000000.002500000000000.00300000000000斜距km2.827416437818912.8251

12、98117297712.820666869662362.814872331059012.806717865362442.79725268974089俯仰角rad*10000.000758504358760.000832822604780.000678082416390.000852790368020.000729007684520.000800724818190.003500000000000.004000000000000.004500000000000.005000000000000.005500000000000.006000000000000.006500000000000.00700

13、0000000000.007500000000000.008000000000000.008500000000000.009000000000000.009500000000000.010000000000000.010500000000000.011000000000000.011500000000000.012000000000000.012500000000000.013000000000000.013500000000000.014000000000000.014500000000000.015000000000000.015500000000000.016000000000000.0

14、16500000000000.017000000000000.017500000000000.018000000000000.018500000000000.019000000000000.019500000000000.020000000000002.786642734750392.773203650263132.759195354645512.743312886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.5903810985078

15、52.562987942728432.534983179507972.506475893722462.475710750163862.445606760009822.414036907720882.382522286116962.350165011823322.317909398371372.285976166564532.254184316814012.222593202195352.192373989694662.162901779972712.134417257937062.108110646907272.083221798231952.061481090267672.042198850

16、940312.026102353143572.012903268635792.004631573883952.000581432519130.000750955762130.000657627253790.000811861485450.000797837270340.000730607129860.000632420065300.000636565244950.000806598456390.000677047400690.000765737677060.000549557590810.000584879139710.000556027473680.000335504125880.00056

17、0126884520.000566944919780.000593806310250.000536819165440.000658719607810.000685983443280.000609224713480.000570860189180.000413085357080.000473020262810.000309493099720.000405526249860.000375450331420.000172823192620.000207583279800.000371864645790.000180821634650.00023323830160-0.000045361869640.

18、000032462840680图2示意图hkV丿其中dk为雷达距离目标的水平距离在物体解：选取系统的状态变量为Idk的自由下落过程中可以认为是常值，hk为物体在k时刻的高度，卩上为物体在k时刻的速度。这里不考虑过程噪声的存在。由牛顿第二运动定律可以建立系统的状态方程为（由采样周T=0.5s，根据离散化的方法得离散的状态方程）d一i00_d-0_k+ikh=0i-0.5h+-0.i25k+ikV00iV0.5k+i1k1X(k+1)=gi00-0_(k+1,k)=0i-0.5，屮(k+1,k)=-0.i2500i0.5其中，g=9.8m/s2测量方程如下：h2+d2k+ik+ilY(k+1)=k

19、+i=9k+1harctan(ui)dk+1+Vk+1设hX(k),k=,；d2(k)+h2(k)th(k)arctgd(k)彎(k),k)|aax(k)x吐)=xhi)d(k)屮d2(k)+h2(k)=-h(k)h(k)vd2(k)+h2(k)d(k)d2(k)+h2(k)d2(k)+h2(k)应用扩展卡尔曼滤波算法可以得到物体的高度和速度随时间变化的最优估计，扩展卡尔曼滤波算法为：一步预测：x(k+1|k)二(k+1,k)X(k|k)+屮(k+1,k)U(k)预测方差：P(k+11k)=(k+1,k)P(k|k)0T(k+1,k)滤波增益：K(k+1)=P(k+11k)Ht(k+1)H(k

20、+1)P(k+1|k)H(k+1)+R(k+1)-i滤波计算：X(k+1|k+1)=F(k+11k)+K(k+1)(Y(k+1)-h(X(k+1|k),k+1)滤波方差：P(k+1|k+1)=IK(k+1)H(k+1)P(k+1|k)由题设，滤波初始值为：1995-500_Ax(0)=2005P(0)=0501002测量误差为：EV(k+1)=0方差R=0.0400.01米用MATLAB软件进行计算，程序清单为:A=100;01-0.5;001;B=0;-0.125;0.5;U=9.80;R=0.040;00.01;C=1000*2.827416437818912.82519811729771

21、2.820666869662362.814872331059012.806717865362442.797252689740892.786642734750392.773203650263132.759195354645512.743312886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.590381098507852.562987942728432.534983179507972.506475893722462.4757107501

22、63860.00075850435876;0.00083282260478;0.00067808241639;0.00085279036802;0.00072900768452;0.00080072481819;0.00075095576213;0.00065762725379;0.00081186148545;0.00079783727034;0.00073060712986;0.00063242006530;0.00063656524495;0.00080659845639;0.00067704740069;0.00076573767706;0.00054955759081;0.00058

23、487913971;0.00055602747368;0.00033550412588;0.00056012688452;2.445606760009822.414036907720882.382522286116962.350165011823322.317909398371372.285976166564532.254184316814012.222593202195352.192373989694662.162901779972712.134417257937062.108110646907272.083221798231952.061481090267672.0421988509403

24、12.026102353143572.012903268635792.004631573883952.000581432519130.00056694491978;0.00059380631025;0.00053681916544;0.00065871960781;0.00068598344328;0.00060922471348;0.00057086018918;0.00041308535708;0.00047302026281;0.00030949309972;0.00040552624986;0.00037545033142;0.00017282319262;0.000207583279

25、80;0.00037186464579;0.00018082163465;0.00023323830160;-0.00004536186964;0.00003246284068;%输入测量数据x=199520051;p=500;050;002;t=0.5:0.5:20;h=zeros(1,length(t);v=zeros(1,length(t);fori=1:length(t)hh(i)=C(i,1)*sin(C(i,2);endfori=1:length(t)x=A*x+B*U;%一步预测p=A*p*A;%一步预测误差方差矩阵H=x(l,l)/sqrt(x(l,l)人2+x(2,l)人2)

26、,x(2,l)/sqrt(x(l,l)人2+x(2,l)人2),0;-x(2,l)/(x(l,l)人2+x(2,l)人2),x(l,l)/(x(l,l)人2+x(2,l)人2),0k=p*H*inv(R+H*p*H);%滤波增益x=x+k*(C(i,:)-sqrt(x(l,l)人2+x(2,l)人2);atan(x(2,l)/x(l,l);%滤波值h(i)=x(2,:);v(i)=x(3,:);%保存高度滤波值%保存速度滤波值p=(eye(3)-k*H)*p;%滤波方差pl(i)=p(l,l);p2(i)=p(2,2);p3(i)=p(3,3);endfigure(l)plot(t,h,r);

27、title(滤波曲线)figure(2),plot(t,hh,b);title(高度测量曲线)figure(3),plot(t,v);title(速度滤波曲线)figure(4),plot(t,p2);title(高度方差曲线)figure(5),plot(t,p3);title(速度方差曲线)disp(h)disp(v)运行程序，得到的物体高度和速度随时间变化的最优估计如下表所示：时间S高度的最优估计km速度的最优估计m/s0.000500000000002.00345.87350.001000000000002.00059.23080.001500000000001.994214.5313

28、0.002000000000001.985819.31810.002500000000001.974524.46420.003000000000001.960929.44280.003500000000001.945234.20750.004000000000001.926739.20850.004500000000001.905944.07740.005000000000001.882748.94110.005500000000001.857053.86490.006000000000001.828858.74180.006500000000001.798063.60800.00700000

29、0000001.765168.51780.007500000000001.729473.40170.008000000000001.691278.29440.008500000000001.650583.18990.009000000000001.607988.09120.009500000000001.562592.99180.010000000000001.513497.90460.010500000000001.4639102.79650.011000000000001.4103107.71270.011500000000001.3549112.61690.012000000000001.2965117.53550.012500000000001.2365122.43640.013000000000001.1740127.33630.013500000000001.1086132.24990.014000000000001.0407137.16490.014500000000000.9709142.06370.015000000000000.8981146.98170.015500000000000.8228151.89680.01