卡尔曼滤波两例题含matlab程序汇总

已知一物体作自由落体运动，对其高度进行了 20 次测量，测量值如下表：时间[s]1234567咼度[km]1.99451.97941.95541.92141.87771.82501.7598时间[s]891011121314咼度[km]1.68671.60361.50921.40761.29441.17241.0399时间[s]151617181920咼度[km]0.89800.74550.58500.41250.23180.0399设高度的测量误差是均值为 0、方差为 1的高斯白噪声随机序列，该物体的初始高度 h 和速度V0Eh1900mh「10也是高斯分布的随机变量，0 =,P=var0=。试求该物体高EV010m/s0V002和速度随时间变化的最优估计。(g 二 9.80m/s2)解：_h(k)—1.令 X(k)二门、t=1R(k)=1Q(k)=0_v(k)_根据离散时间卡尔曼滤波公式，则有：X(k+1)=©(k+1,k)X(k)+U(k)Y(k+1)=H(k+1)X(k+1)+V(k+1)100P(010)二 var[X(0)]二 P 二0步预测：文(k+11k)=©(k+1,k)文(kIk)+U(k)P(k+11k)=©(k+1,k)P(kIk)©T(k+1,k)滤波增益：K(k+1)=P(k+11k)HT(k+1)[H(k+1)P(k+11k)HT(k+1)+R(k+1)]-1AAA滤波计算：X(k+11k+1)=X(k+11k)+K(k+1)[Y(k+1)-H(k+1)X(k+11k)]P(k+11k+1)=[I-K(k+1)H(k+1)]P(k+11k)2.实验结果Q(k+1,k)=U(k)=-05gt2gt滤波初值：AX(0 丨 0)二 EX(0)二1900101高度随时间变化估计高度滤波值速度随时间变化的最优估计高度协方差速度协方差从以上的结果，可以得到高度和速度的估计值，再通过所得到的高度协方差和速度协方差，可见用卡尔曼滤波法，虽然刚开始的初始高度协方差很大为 100,但通过 2 步之后减小到不超过1,逐渐接近于 0，同样的速度协方差刚开始的时候也比较大，为 2,但是通过 5 步之后迅速减小,到 10 步之后接近于 0。3.有关参数的影响（例如初始条件、噪声统计特性对滤波结果的影响等）;速度度协方差1）初始条件改变时，改变初始高度值，和速度值一Eh2300mEV30m/s1-0」1——1高度徳液值速度滤彼值由实验结果分析可得度滤波值和速度滤波值在开始几步接近初始值，协方差值基本不变。h2）当初始协方差值改变时，改为 P=varo0V1-0」1500010实验结果分析高度和速度滤波值基本不变，速度协方差和高度协方差开始要接近速度协方差和高度协方差的初始值。但是经过几步之后，都趋于 0。.同样考虑自由落体运动的物体，用雷达（和物体落地点在同一水平面）进行测量，如图所物体的高度和速度随时间变化的估计值。时间[s]*10000.000500000000000.001000000000000.00150000000000斜距[km]2.827416437818912.825198117297712.82066686966236 俯仰角[rad]*10000.000758504358760.00083...