1、1随机系统滤波与控制大作业姓名：史贝贝学号：3107040016班级：硕 736专业：检测技术与自动化装置21. 已知一物体作自由落体运动，对其高度进行了 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.
2、0399设高度的测量误差是均值为 0、方差为 1 的高斯白噪声随机序列，试求该物体高度和速度随时间变化的最优估计。 （ ）2/8.9smg解：(1).理论分析设 时刻的位移、速度、加速度和加加速度分别为 。对运动物体的跟踪kt ,kkhvaj者来说， 是随机量，此处取为白噪声。j由运动学方程可得：2111nkkkkaThvaj观测方程 kkzhN其中 。 （题目中所 给高1,0,0,TTkllkklklTsENDEjDj度数据单位为 Km,应先将其化为单位 m）.取状态向量kkhxva则状态方程为 1kj观测方程为 k kzhNHx其中 。21/00,10kTT可见，这是随机线性定常系统的滤波
3、问题。应用 Kalman 基本滤波方程式，有311, ,1kkkTkkxKzHxPI初始条件的选取：1043.5,(0)19.8Px中速度是运动学方程得出。如果系统是稳定的，那么初始条件的选取对结果的影响很(0)x小。从以后的实验结果可以看出。滤波方差最终稳定在某一数值上，系统是稳定的。(2).计算结果与结果分析1).计算得物体高度随时间变化的最优估计可见用卡尔曼滤波法得到的高度滤波值在初始时与理想值有一定误差，在递推步数 k增大时，误差值越来越小2).计算得物体速度随时间变化的最优估计4可见用卡尔曼滤波法得到的速度滤波值在初始时与理想值有一定误差，在递推步数 k增大时，误差值越来越小3).高
4、度协方差可见用卡尔曼滤波法得到的高度协方差在初始时较大，为 100，在递推步数 k3 时不超过 2。4).速度协方差5可见用卡尔曼滤波法得到的速度协方差在初始时较大为 2，在递推步数 k5 时，不超过 0.2，k10 时接近于 0。(3).源程序%卡尔曼滤波算法在估计自由落体轨迹中的应用%输入观测值,此处对高度H做出观测Z=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.0 745.5 585.0 412.5 231.8 39.9;%初始状
5、态向量X=1993.5;-10;-9.8;%初始方差矩阵D=10 0 0;0 10 0;0 0 1;%观测间隔T=1;%状态方程系数矩阵fea=1 T T2/2; 0 1 T; 0 0 1;tao=0;0;T;H=1 0 0;%噪声方差r=1; %观测噪声q=1; %过程噪声%调用子函数KALMANtempX,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(滤波前, 滤波后)6figure(2)pl
6、ot(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,tao,H,r,q)%子程序返回值：3个状态变量中间值， 3个状态变量滤波方差变化n=length(Z); %计算观测值长度I=eye(3);%下面定义的变量用于存储计算过程滤波方差变化var
7、H=zeros(1,n); %高度方差varV=zeros(1,n); %速度方差varA=zeros(1,n); %加速度方差%变量tempX 用于存储状态变化tempX=zeros(3,n);for j=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);X=fea*X+K*(Z(1,j)-H*fea*X);D=(I-K*H)*tempD;end72. 同样考虑自由落体运动的物体，用雷达（和物体落地点在
8、同一水平面）进行测量，如图所示。如果雷达测距和测角的测量噪声是高斯白噪声随机序列，均值为零、方差阵，试根据下列测量数据确定物体的高度和速度随时间变化的估计值。01.4.R时间s*1000 斜距km 俯仰角rad*10000.00050000000000 2.82741643781891 0.000758504358760.00100000000000 2.82519811729771 0.000832822604780.00150000000000 2.82066686966236 0.000678082416390.00200000000000 2.81487233105901 0.0008
9、52790368020.00250000000000 2.80671786536244 0.000729007684520.00300000000000 2.79725268974089 0.000800724818190.00350000000000 2.78664273475039 0.000750955762130.00400000000000 2.77320365026313 0.000657627253790.00450000000000 2.75919535464551 0.000811861485450.00500000000000 2.74331288628195 0.0007
10、97837270340.00550000000000 2.72538888482812 0.000730607129860.00600000000000 2.70664967712312 0.000632420065300.00650000000000 2.68632403406473 0.000636565244950.00700000000000 2.66386533852220 0.000806598456390.00750000000000 2.64093529707333 0.000677047400690.00800000000000 2.61621111727357 0.0007
11、65737677060.00850000000000 2.59038109850785 0.000549557590810.00900000000000 2.56298794272843 0.000584879139710.00950000000000 2.53498317950797 0.000556027473680.01000000000000 2.50647589372246 0.000335504125880.01050000000000 2.47571075016386 0.000560126884520.01100000000000 2.44560676000982 0.0005
12、66944919780.01150000000000 2.41403690772088 0.000593806310250.01200000000000 2.38252228611696 0.000536819165440.01250000000000 2.35016501182332 0.000658719607810.01300000000000 2.31790939837137 0.000685983443280.01350000000000 2.28597616656453 0.000609224713480.01400000000000 2.25418431681401 0.0005
13、70860189180.01450000000000 2.22259320219535 0.000413085357080.01500000000000 2.19237398969466 0.000473020262810.01550000000000 2.16290177997271 0.000309493099720.01600000000000 2.13441725793706 0.000405526249860.01650000000000 2.10811064690727 0.000375450331420.01700000000000 2.08322179823195 0.0001
14、72823192620.01750000000000 2.06148109026767 0.000207583279800.01800000000000 2.04219885094031 0.000371864645790.01850000000000 2.02610235314357 0.0001808216346580.01900000000000 2.01290326863579 0.000233238301600.01950000000000 2.00463157388395 -0.000045361869640.02000000000000 2.00058143251913 0.00
15、003246284068解：(1).理论分析应该选取高度 h(t)，速度 v(t)，d 为状态变量 x(t)，即： 。()dXthtv根据自由落体系统的机理，选取向上为正方向，则有：()vtgd()httd即： .00()1()dxtXtg根据连续时间描述离散化的相关知识， ， 。则有：01F0Cg(,)()0()1tItt雷达物体Vhd0斜距俯仰角题 2 示意图9故：10(,)kT(1) 2(,),(0.5KTkCdgt故系统的状态方程为： 210()()0.5XkTXkgt又因为给定的量测值是斜距，俯仰角，故系统的量测方程为： 22(1)(1)() (1)arctnhkdLkYVk辅助方程
16、： (1)|)(),(1)|xkkhXkHk 22 2222(1|)0(1|)|()(1|)dhkhkddhk 则系统量测方程变为： (1)()1()YkHXkV题目要求给出该物体高度和速度随时间变化的的最优估计值，并且给定了斜距，俯仰角测量值，故可以用卡尔曼滤波解决问题。T=0.5 Q(k)=0 0.4().1Rk()lkY滤波初值： 195(0|)2X5(|)2P一步预测： (1|)(,)(|()kkXkU| |1,TPP滤波增益： 1()(|)()()|)(1)()TKkkHkPkHRk10滤波计算： (1|)(1|)()1(|),1XkXkKkYhXk|)PIHP(2).计算结果与结果分析1).高度滤波值可见用扩展卡尔曼滤波法得到的高度滤波值与理想值误差很小。2).速度滤波值可见用扩展卡尔曼滤波法得到的速度滤波值与理想值在初始几步有一些误差，再 k5时误差很小。3). 高度协方差
