已知一物体作自由落体运动对其高度进行了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.0399设高度的测量误差是均值为 0、方差为1的高斯白噪声随机序列， 该物体的初始高度 h0和速度 V也是高斯分布的随机变量，且Eh。= |1900mP0 = vaqho= 100 0 l试EV。

2、l0m/s0V。一 o 2一求该物体高度和速度随时间变化的最优估计。(g = 9.80m/s2)解：选取系统的状态变量为 hv vk T， hk为物体在k时刻的高度，vk为物体在k时刻的瞬时速度。这里不考虑过程噪声的存在。(由采样周T=1s,根据离散化的方法得由牛顿第二运动定律可以写出系统的状态方程: 离散的状态方程)X(k 1其中>(k 1,k)=v(k 1,k)-0.51N1 J建立如下的测量方程:ihk 1丫 (k+1) = 1 0|+Vk 卅W一 其中 H(k 1) = 1 0 1。应用卡尔曼滤波算法可以得到物体的高度和速度随时间变化的最优估计，卡尔曼滤波算法 为：一步预测:Xk

3、 + 1k)=(k + 1,k)X"(k k) + 甲(k + 1,k)U(k)预测方差:P(k +1k) =6(k+1,k)P(kk) T(k+1, k)滤波增益:K(k +1) =P(k +1k)HT(k +1)H(k+1)P(k +1k)H T(k+1) + R(k + 1)滤波计算：P)?(k 1k 1 )?(k 1k) K(k 1)Y(k 1) - H (k 1)>?(k 1k) 滤波方差：P(k 1k 1) =l -K(k 1)H(k 1)P(k 1k)由题设，滤波初始值为：AX(0)=Eh0_ 1900mEV0 _ 10m/sP0 =Varh0100 0bHo 2

4、测量误差为：EV( k+1)=0, Var V(k+1)=1采用MATLAB软件进行计算，程序清单为:A=1 -1;0 1;B=-1/2;1;C=1 0;U=9.80; R=1;h仁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 745.5 585 412.5 231.8 39.9;x=1900 10'p=100 0;0 2;t=1:20;he=zeros(1,le ngth(t);for i=1:20%步预测%一步预测误差方差矩阵

5、%滤波增益%滤波值%保存高度滤波值%保存速度滤波值%滤波方差x=A*x+B*U;p=A*p*A： k=p*C'*i nv(R+C*p*C'); x=x+k*(h1(i)-C*x);h2(i)=x(1,:);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

6、 (2),plot(t,v');lege nd(速度曲线') title('速度曲线') figure(3)plot(P1,'r');lege nd('高度方差') title('高度方差') figure(4)plot(P2,'r')lege nd(速度方差') title('速度方差')运行程序，得到的物体高度和速度随时间变化的最优估计如下表所示:时间s测量高度m高度的最优估计m速度的最优估计m/s11994.51993.417.67621979.41977.323.23

7、831955.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.05156.7417585.0584.54166.5318412.5412.98176.

8、3419231.8231.76186.142039.940.566195.95上表为物体随时间变化的高度量测值和高度，速度的最优估计值 按此计算结果绘制的曲线图如下：滤液曲统和测量曲钱涯液曲蝮和测虽曲議20001* *7 I1111l一II_ |入*滤波柑找1B0D -、*初薑曲线|一I1EOD -*-1JQD -+-X1200 - * _1D0D -丸-90060D -羊-40D -200 -I - Ik0 IIIiiillII"FD 2 J B B 1012 U 161020结果分析：A此题中我们利用初值 X (0)和P(0)根据卡尔曼滤波算法递推的求出了物体的高度和速 度的最优

9、估计，从物体的高度的最优估计曲线中我们可以看到，物体的高度最优估计值和实际测量值几乎相等，物体的速度的最优估计也和自由落体的速度几乎一致，只是在初始滤波时有一定的的偏差，这是由于滤波初值选取决定的。在许多实际问题中，往往不能精确知道 初值。很显然此时计算得到的滤波值将不是最优的。根据相应的判定准则，我们知道该系统是一致完全能和一直完全能观的。即它的最优线性最优滤波系统是一直渐近稳定的，所以滤波初值选取只影响滤波的初始阶段，系统的滤波最终会趋向稳定。这是有系统自身的特性决定的。因此初值任意选取并不会影响滤波的最后阶段的结果。显然，上述结果是在测量误差矩阵R精确可知的情况下得到的。如果R不精确，那

10、么由上A述算法给出的X (k)将不是X(k)的最优估计。并且此时，滤波还可能发散。这时我们可以采 用噪声不精确（未知）系统的自适应滤波：可以先选取一个适当的量测噪声矩阵并将它固定 下来，然后按照动态噪音不精确的情况设计自适应滤波。本题中假设测量误差为均值为 0,方差为1的高斯白色噪声随机序列，并且初始高度和速度相互独立。显然这种假设是比较理想的状况。如果测量误差为有色噪声，此时我们可以通过适当的变换把系统方程中的有色噪声转变成白色噪声的情形处理，从而得到一个白色噪声作用下的离散线性系统，有此系统的滤波算法导出原系统的滤波算法。由上图可以看出，速度的滤波方差在最初的时间内很大（接近于2），高度滤

11、波方差在初始时更大（接近于100），但是很快速度滤波方差就收敛到 0；而高度的滤波方差收敛速度 比速度滤波方差的要慢，但是最终也收敛到了某个稳态值。改变参数看影响：将滤波初值、初值方差、测量噪声方差分别变化，其相应滤波方差如图所示：滤浪初值增加时高度方差和遠度方差爾波初值减少时高度方菱和速度方菱初值方差增加时高度方差和速度方差初值方差减少时高度方差和速度方差测宗噪声增加B扌高度方差和速度方差测量噪声械少时高度方差和速度方差所示。Ed。刁995mEh。=2005m，P0 = varhEV0 一i1m/s1 1v。一如果50声是高斯白噪声随机序列，均值为零、方差阵试根据下列测量数据确0.01综上所

12、示，滤波初值的改变对滤波方差影响不大（其实这一点在卡尔曼滤波算法中也可看出），而改变初值方差、测量噪声方差对滤波方差有较大的影响。初值方差和测量噪声方 差的增加都会使高度方差和速度方差变大，且收敛速度变慢；相应地，他们的减少也都会使高度方差和速度方差变小，收敛速度变快。从统计学的角度讲，方差代表数据的精度， 方差大则数据偏离真值副度就大，数据本身的可靠性就差，数据所带来的信息就小，所带来的误差就大（根据误差理论的误差可传递性）2、同样考虑自由落体运动的物体，用雷达（和物体落地点在同一水平面）进行测量，如图0 050 ，且雷达测距和测角的测量噪0 2定物体的高度和速度随时间变化的估计值。时间s*

13、10000.000500000000000.001000000000000.001500000000000.002000000000000.002500000000000.00300000000000斜距km2.827416437818912.825198117297712.820666869662362.814872331059012.806717865362442.79725268974089 俯仰角rad*10000.000758504358760.000832822604780.000678082416390.000852790368020.000729007684520.000800

14、724818190.003500000000000.004000000000000.004500000000000.005000000000000.005500000000000.006000000000000.006500000000000.007000000000000.007500000000000.008000000000000.008500000000000.009000000000000.009500000000000.010000000000000.010500000000000.011000000000000.011500000000000.012000000000000.01

15、2500000000000.013000000000000.013500000000000.014000000000000.014500000000000.015000000000000.015500000000000.016000000000000.016500000000000.017000000000000.017500000000000.018000000000000.018500000000000.019000000000000.019500000000000.020000000000002.786642734750392.773203650263132.75919535464551

16、2.743312886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.590381098507852.562987942728432.534983179507972.506475893722462.475710750163862.445606760009822.414036907720882.382522286116962.350165011823322.317909398371372.285976166564532.2541843168

17、14012.222593202195352.192373989694662.162901779972712.134417257937062.108110646907272.083221798231952.061481090267672.042198850940312.026102353143572.012903268635792.004631573883952.000581432519130.00075095576213 0.00065762725379 0.00081186148545 0.00079783727034 0.00073060712986 0.00063242006530 0.

18、00063656524495 0.00080659845639 0.00067704740069 0.00076573767706 0.00054955759081 0.00058487913971 0.00055602747368 0.00033550412588 0.00056012688452 0.00056694491978 0.00059380631025 0.00053681916544 0.00065871960781 0.00068598344328 0.00060922471348 0.00057086018918 0.00041308535708 0.00047302026

19、281 0.00030949309972 0.00040552624986 0.00037545033142 0.00017282319262 0.00020758327980 0.00037186464579 0.00018082163465 0.00023323830160 -0.000045361869640.00003246284068物体俯仰角、雷达do图2示意图解：选取系统的状态变量为 dkhk VJ，其中dk为雷达距离目标的水平距离，在物体的自由下落过程中可以认为是常值,hk为物体在k时刻的高度，Vk为物体在k时刻的速度。这里不考虑过程噪声的存在。由牛顿第二运动定律可以建立系统的

20、状态方程为: 法得离散的状态方程）（由采样周T=0.5s，根据离散化的方测量方程如下:设 hX(k),k=X(k 1)二W! _1hk +50 dk_0-0.5-0.1250.5100 1-01-0.5，普(k+1,k) =-0.125卫01 _1 10.5 一01，g其中 ：（k 1,k）二曲+dk卅1hk+i、2=9.8m/ sY(k +1)=卜 | h41 卅larcta n()-dk4t _Jd2(k) +h2(k) arctg 型- d(k) 一d(k)Vk1h(k)k）=血決（k）.d2(k) h2(k). d2(k) h2(k)-h(k)d2(k) h2(k)d(k)d2(k)

21、h2(k)00扩展卡尔曼滤应用扩展卡尔曼滤波算法可以得到物体的高度和速度随时间变化的最优估计， 波算法为：一步预测：X(k 1|k)二G(k 1,k)?(k|k) "k 1,k)U(k) 预测方差：P(k 1k)- ：:(k 1,k)P(kk)： J(k 1, k) 滤波增益：K k 1 二 P(k 1|k)HT(k 1)H(k 1)P(k 1k)HT(k 1) R(k 1)J 滤波计算：Xk +1|k +1) = >?(k +1|k) + K(k +1)(Y(k +1) h(Xk +1|k), k + 1) 滤波方差：P(k 1k 1) =l -K(k 1)H(k 1)P(k

22、 1|k)由题设，滤波初始值为：a一1995"5 0 0X(0) = 2005 P( 0>0 5 0Ji P 0 2测量误差为：0.040 1EV( k+1)=0,方差 R= I0 0.01 一采用MATLAB软件进行计算，程序清单为:A=1 0 0;0 1 -0.5;0 0 1;B=0;-0.125;0.5; U=9.80; R=0.04 0;0 0.01;C=1000*2.827416437818912.825198117297712.820666869662362.814872331059012.806717865362442.797252689740892.786642

23、734750392.773203650263132.759195354645512.743312886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.590381098507852.562987942728432.534983179507972.506475893722462.475710750163860.00075850435876;0.00083282260478;0.00067808241639;0.00085279036802;

24、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.00058487913971;0.00055602747368;0.00033550412588;0.00056012688452;2.445606760

25、009822.414036907720882.382522286116962.350165011823322.317909398371372.285976166564530.00056694491978;0.00059380631025;0.00053681916544;0.00065871960781;0.00068598344328;0.00060922471348;2.254184316814012.222593202195352.192373989694660.00057086018918;0.00041308535708;0.00047302026281;2.162901779972

26、712.134417257937062.108110646907272.083221798231952.061481090267672.042198850940312.026102353143570.00030949309972;0.00040552624986;0.00037545033142;0.00017282319262;0.00020758327980;0.00037186464579;0.00018082163465;2.012903268635792.004631573883952.000581432519130.00023323830160;-0.00004536186964;

27、0.00003246284068; %输入测量数据x=1995 2005 1'p=5 0 0;0 5 0;0 0 2;t=0.5:0.5:20;h=zeros(1,length(t);v=zeros(1,length(t); for i=1:length(t) hh(i)=C(i,1)*sin(C(i,2); end for i=1:length(t)x=A*x+B*U;%一步预测p=A*p*A'% 一步预测误差方差矩阵H=x(1,1)/sqrt(x(1,1)A2+x(2,1)A2),x(2,1)/sqrt(x(1,1)A2+x(2,1)A2),0; -x(2,1)/(x(1,

28、1)A2+x(2,1)A2),x(1,1)/(x(1,1F2+x(2,1)A2),0 k=p*H'*inv(R+H*p*H');% 滤波增益x=x+k*(C(i,:)'-sqrt(x(1,1)A2+x(2,1)A2);ata n(x(2,1)/x(1,1);% 滤波值h(i)=x(2,:);%保存高度滤波值v(i)=x(3,:);%保存速度滤波值p=(eye(3)-k*H)*p;% 滤波方差p1(i)=p(1,1); p2(i)=p(2,2);p3(i)=p(3,3);endfigure(1) plot(t,h,'r'); title(' 滤波曲

29、线 ') 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.000500000000

30、002.00345.87350.001000000000002.00059.23080.001500000000001.994214.53130.002000000000001.985819.31810.002500000000001.974524.46420.003000000000001.960929.44280.003500000000001.945234.20750.004000000000001.926739.20850.004500000000001.905944.07740.005000000000001.882748.94110.005500000000001.857053.8

31、6490.006000000000001.828858.74180.006500000000001.798063.60800.007000000000001.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.