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

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

《1已知一物体作自由落体运动对其高度进行了20次测量.docx》由会员分享，可在线阅读，更多相关《1已知一物体作自由落体运动对其高度进行了20次测量.docx（23页珍藏版）》请在冰豆网上搜索。

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

1、已知一物体作自由落体运动，对其高度进行了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的高斯白噪声随机序列，该物体的初始高度

和速度

也是高斯分布的随机变量，且

。

试求该物体高度和速度随时间变化的最优估计。

（

）

解：

选取系统的状态变量为

，

为物体在k时刻的高度，

为物体在k时刻的瞬时速度。

这里不考虑过程噪声的存在。

由牛顿第二运动定律可以写出系统的状态方程：

（由采样周T=1s，根据离散化的方法得离散的状态方程）

其中

，

建立如下的测量方程：

其中

。

应用卡尔曼滤波算法可以得到物体的高度和速度随时间变化的最优估计，卡尔曼滤波算法为：

一步预测：

预测方差：

滤波增益：

滤波计算：

P

滤波方差：

由题设，滤波初始值为：

=

测量误差为：

EV（k+1）=0,VarV（k+1）=1

采用MATLAB软件进行计算，程序清单为：

A=[1-1;01];B=[-1/2;1];C=[10];U=9.80;R=1;

h1=[1994.51979.41955.41921.41877.71825.01759.81686.71603.61509.21407.61294.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,:

）;%保存高度滤波值

v（i）=x（2,:

）;%保存速度滤波值

p=（eye

（2）-k*C）*p;%滤波方差

P1（i）=p（1,1）

P2（i）=p（2,2）

end

figure

（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]

速度的最优估计[m/s]

1

1994.5

1993.4

17.676

2

1979.4

1977.3

23.238

3

1955.4

1953.7

30.55

4

1921.4

1920.3

39.5

5

1877.7

1877

48.964

6

1825.0

1824.1

58.5

7

1759.8

1760.3

68.391

8

1686.7

1686.9

78.214

9

1603.6

1603.7

88.024

10

1509.2

1510.2

97.909

11

1407.6

1407.5

107.7

12

1294.4

1294.7

117.52

13

1172.4

1172.3

127.32

14

1039.9

1040.1

137.12

15

898.0

898.03

146.92

16

745.5

746.05

156.74

17

585.0

584.54

166.53

18

412.5

412.98

176.34

19

231.8

231.76

186.14

20

39.9

40.566

195.95

上表为物体随时间变化的高度量测值和高度，速度的最优估计值

按此计算结果绘制的曲线图如下：

结果分析：

此题中我们利用初值

和P（0）根据卡尔曼滤波算法递推的求出了物体的高度和速度的最优估计，从物体的高度的最优估计曲线中我们可以看到，物体的高度最优估计值和实际测量值几乎相等，物体的速度的最优估计也和自由落体的速度几乎一致，只是在初始滤波时有一定的的偏差，这是由于滤波初值选取决定的。

在许多实际问题中，往往不能精确知道初值。

很显然此时计算得到的滤波值将不是最优的。

根据相应的判定准则，我们知道该系统是一致完全能和一直完全能观的。

即它的最优线性最优滤波系统是一直渐近稳定的，所以滤波初值选取只影响滤波的初始阶段，系统的滤波最终会趋向稳定。

这是有系统自身的特性决定的。

因此初值任意选取并不会影响滤波的最后阶段的结果。

显然，上述结果是在测量误差矩阵R精确可知的情况下得到的。

如果R不精确，那么由上述算法给出的

将不是X（k）的最优估计。

并且此时，滤波还可能发散。

这时我们可以采用噪声不精确（未知）系统的自适应滤波：

可以先选取一个适当的量测噪声矩阵并将它固定下来，然后按照动态噪音不精确的情况设计自适应滤波。

本题中假设测量误差为均值为0，方差为1的高斯白色噪声随机序列，并且初始高度和速度相互独立。

显然这种假设是比较理想的状况。

如果测量误差为有色噪声，此时我们可以通过适当的变换把系统方程中的有色噪声转变成白色噪声的情形处理，从而得到一个白色噪声作用下的离散线性系统，有此系统的滤波算法导出原系统的滤波算法。

由上图可以看出，速度的滤波方差在最初的时间内很大（接近于2），高度滤波方差在初始时更大（接近于100），但是很快速度滤波方差就收敛到0；而高度的滤波方差收敛速度比速度滤波方差的要慢，但是最终也收敛到了某个稳态值。

改变参数看影响：

将滤波初值、初值方差、测量噪声方差分别变化，其相应滤波方差如图所示：

综上所示，滤波初值的改变对滤波方差影响不大（其实这一点在卡尔曼滤波算法中也可看出），而改变初值方差、测量噪声方差对滤波方差有较大的影响。

初值方差和测量噪声方差的增加都会使高度方差和速度方差变大，且收敛速度变慢；相应地，他们的减少也都会使高度方差和速度方差变小，收敛速度变快。

从统计学的角度讲，方差代表数据的精度，方差大则数据偏离真值副度就大，数据本身的可靠性就差，数据所带来的信息就小，所带来的误差就大（根据误差理论的误差可传递性）。

2、同样考虑自由落体运动的物体，用雷达（和物体落地点在同一水平面）进行测量，如图所示。

如果

，

，且雷达测距和测角的测量噪声是高斯白噪声随机序列，均值为零、方差阵

，试根据下列测量数据确定物体的高度和速度随时间变化的估计值。

时间[s]*1000斜距[km]俯仰角[rad]*1000

0.000500000000002.827416437818910.00075850435876

0.001000000000002.825198117297710.00083282260478

0.001500000000002.820666869662360.00067808241639

0.002000000000002.814872331059010.00085279036802

0.002500000000002.806717865362440.00072900768452

0.003000000000002.797252689740890.00080072481819

0.003500000000002.786642734750390.00075095576213

0.004000000000002.773203650263130.00065762725379

0.004500000000002.759195354645510.00081186148545

0.005000000000002.743312886281950.00079783727034

0.005500000000002.725388884828120.00073060712986

0.006000000000002.706649677123120.00063242006530

0.006500000000002.686324034064730.00063656524495

0.007000000000002.663865338522200.00080659845639

0.007500000000002.640935297073330.00067704740069

0.008000000000002.616211117273570.00076573767706

0.008500000000002.590381098507850.00054955759081

0.009000000000002.562987942728430.00058487913971

0.009500000000002.534983179507970.00055602747368

.010*********

.010*********

0.011000000000002.445606760009820.00056694491978

0.011500000000002.414036907720880.00059380631025

0.012000000000002.382522286116960.00053681916544

0.012500000000002.350165011823320.00065871960781

0.013000000000002.317909398371370.00068598344328

0.013500000000002.285976166564530.00060922471348

0.014000000000002.254184316814010.00057086018918

0.014500000000002.222593202195350.00041308535708

0.015000000000002.192373989694660.00047302026281

0.015500000000002.162901779972710.00030949309972

0.016000000000002.134********7060.00040552624986

0.016500000000002.108110646907270.00037545033142

0.017000000000002.083221798231950.00017282319262

0.017500000000002.061481090267670.00020758327980

0.018000000000002.042198850940310.00037186464579

0.018500000000002.026102353143570.00018082163465

0.019000000000002.012903268635790.00023323830160

0.019500000000002.00463157388395-0.00004536186964

.020*********

图2示意图

解：

选取系统的状态变量为

，其中

为雷达距离目标的水平距离，在物体的自由下落过程中可以认为是常值，

为物体在k时刻的高度，

为物体在k时刻的速度。

这里不考虑过程噪声的存在。

由牛顿第二运动定律可以建立系统的状态方程为：

（由采样周T=0.5s，根据离散化的方法得离散的状态方程）

其中

，

，

测量方程如下：

设h[X（k）,k]=

H（k）=

=

]

应用扩展卡尔曼滤波算法可以得到物体的高度和速度随时间变化的最优估计，扩展卡尔曼滤波算法为：

一步预测：

预测方差：

滤波增益：

滤波计算：

滤波方差：

由题设，滤波初始值为：

测量误差为：

EV（k+1）=0,方差

采用MATLAB软件进行计算，程序清单为：

A=[100;01-0.5;001];B=[0;-0.125;0.5];

U=9.80;R=[0.040;00.01];

C=1000*[2.827416437818910.00075850435876;

2.825198117297710.00083282260478;

2.820666869662360.00067808241639;

2.814872331059010.00085279036802;

2.806717865362440.00072900768452;

2.797252689740890.00080072481819;

2.786642734750390.00075095576213;

2.773203650263130.00065762725379;

2.759195354645510.00081186148545;

2.743312886281950.00079783727034;

2.725388884828120.00073060712986;

2.706649677123120.00063242006530;

2.686324034064730.00063656524495;

2.663865338522200.00080659845639;

2.640935297073330.00067704740069;

2.616211117273570.00076573767706;

2.590381098507850.00054955759081;

2.562987942728430.00058487913971;

2.534983179507970.00055602747368;

2.506475893722460.00033550412588;

2.475710750163860.00056012688452;

2.445606760009820.00056694491978;

2.414036907720880.00059380631025;

2.382522286116960.00053681916544;

2.350165011823320.00065871960781;

2.317909398371370.00068598344328;

2.285976166564530.00060922471348;

2.254184316814010.00057086018918;

2.222593202195350.00041308535708;

2.192373989694660.00047302026281;

2.162901779972710.00030949309972;

2.134417257937060.00040552624986;

2.108110646907270.00037545033142;

2.083221798231950.00017282319262;

2.061481090267670.00020758327980;

2.042198850940310.00037186464579;

2.026102353143570.00018082163465;

2.012903268635790.00023323830160;

2.00463157388395-0.00004536186964;

2.000581432519130.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））;

end

fori=1:

length（t）

x=A*x+B*U;%一步预测

p=A*p*A';%一步预测误差方差矩阵

H=[x（1,1）/sqrt（x（1,1）^2+x（2,1）^2）,x（2,1）/sqrt（x（1,1）^2+x（2,1）^2）,0;

-x（2,1）/（x（1,1）^2+x（2,1）^2）,x（1,1）/（x（1,1）^2+x（2,1）^2）,0]

k=p*H'*inv（R+H*p*H'）;%滤波增益

x=x+k*（C（i,:

）'-[sqrt（x（1,1）^2+x（2,1）^2）;atan（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）;

end

figure

（1）

plot（t,h,'r'）;

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/s]

0.00050000000000

2.0034

5.8735

0.00100000000000

2.0005

9.2308

0.00150000000000

1.9942

14.5313

0.00200000000000

1.9858

19.3181

0.00250000000000

1.9745

24.4642

0.00300000000000

1.9609

29.4428

0.00350000000000

1.9452

34.2075

0.00400000000000

1.9267

39.2085

0.00450000000000

1.9059

44.0774

0.00500000000000

1.8827

48.9411

0.00550000000000

1.8570

53.8649

0.00600000000000

1.8288

58.7418

0.00650000000000

1.7980

63.6080

0.00700000000000

1.7651

68.5178

0.00750000000000

1.7294

73.4017

0.00800000000000

1.6912

78.2944

0.00850000000000

1.6505

83.1899

0.00900000000000

1.6079

88.0912

0.00950000000000

1.5625

92.9918

.010*********

1.5134

97.9046

.010*********

1.4639

102.7965

0.01100000000000

1.4103

107.7127

0.01150000000000

1.3549

112.6169

0.01200000000000

1.2965

117.5355

0.01250000000000

1.2365

122.4364

0.01300000000000

1.1740

127.3363

0.01350000000000

1.1086

132.2499

0.01400000000000

1.0407

137.1649

0.01450000000000

0.9709

142.0637

0.01500000000000

0.8981

146.9817

0.01550000000000

0.8228

151.8968

0.01600000000000

0.7458

156.7927

0.01650000000000

0.6660

161.6981

0.01700000000000

0.5843

166.5855

0.01750000000000

0.4996

171.4905

0.01800000000000

0.4125

176.3956

0.01850000000000

0.3229

181.3015

0.01900000000000

0.2315

186.1875

0.01950000000000

0.1371

191.0887

.020*********

0.0401

195.9957

上表为物体随时间变化的高度量测值和高度，速度的最优估计值

按此计算结果绘制的曲线图如下：

结果分析：

此系统为一非线性系统，为此我们采用了扩展卡尔曼滤波器。

其基本思想是：

以非线性系统的状态X的已知滤波估值为参考点，把非线性向量f[.]和h[.]在

附近展开成台劳级数。

取其一次项得到X满足的线性化方程，利用一般离散线性系统的的卡尔曼滤波算法导出有限范围内的近似滤波估值算法。

此题中，我们也是应用利用初值

和P（0）根据扩展卡尔曼滤波算法递推的求出了物体的高度和速度的最优估计。

从物体的高度的最优估计曲线中我们可以看到，物体的高度最优估计值随时间变化的曲线和自由落体的比较接近。

速度最优估计值随时间变化的曲线也与自由落体的比较接近。

由于系统本身是非线性系统，我们用线性化的方法省去了台劳展开式的高次向，则必然引入非线性误差，这一点从高度方差曲线中可明显看出。

改变参数看影响：

将初值方差、测量噪声方差分别变化，其相应滤波方差如图所示：

综上