Kalman滤波原理及程序（手册）解析(2)

所示。

15真实值EKF估计值 8761055043-52-101-15 051015202530354045500

05101520253035404550

图3-2-1 EKF滤波处理后的状态与真值对比 图3-2-2 偏差分析 EKF一维非线性系统仿真程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 函数功能：一维非线性系统扩展Kalman滤波问题

% 状态函数：X(k+1)=0.5X(k)+2.5X(k)/(1+X(k)^2)+8cos(1.2k) +w(k) % 观测方程：Z（k）=X(k)^2/20 +v(k)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function EKF_for_One_Div_UnLine_System % 初始化

T=50; % 总时间 Q=10; R=1;

% 产生过程噪声 w=sqrt(Q)*randn(1,T); % 产生观测噪声 v=sqrt(R)*randn(1,T);

…… ……

四、EKF在纯方位寻的导弹制导中的应用例子：

考虑一个在三维平面x-y-z内运动的质点M，其在某一时刻k的位置、速度和加速度可用矢量可以表示为：

x(k)?rx(k)ry(k)rz(k)vx(k)vy(k)vz(k)ax(k)ay(k)az(k)

??T质点M可以在三维空间内做任何运动，同时假设三个x-y-z方向上运动具有加性系统噪声w(k)，则在笛卡尔坐标系下该质点的运动状态方程为：

x(k?1)?fk(x(k),w(k))

通常情况下，上述方程为线性的，即能表示为以下方式，

x(k?1)??x(k)??u(k)?w(k)

其中

??I3????03??03???tI3I303????t(e???t?1)I3??2??(?t2/2)I3??1?????t?，?????tI3? (1?e)I3???????t03eI3?????1?t为测量周期，也叫扫描周期，采样时间间隔等。动态噪声w(k)为

w(k)?000000?x(k)?y(k)?z(k)

??T而且

06?3??0 E?w(k)??q1?09?1，Ew(k)wT(k)?Q1??6?2?03?6?I3???w(k)是高斯型白色随机向量序列。

现在考虑一个带有观测器的飞行中的导弹，可以假设为质点M，对移动的目标进行观测，如下图所示，导弹与目标的相对位置依然可用x-y-z表示，

那么，导弹对目标纯方位角观测，主要是俯仰角和水平方向偏向角，实际测量中雷达具有加性测量噪声v(k)，则在笛卡尔坐标系下，观测方程为

z(k)?h?x(k)??v(k)

式中，

?ry(k)h?x(k)???arctan22?r(k)?rxz(k)??rx(k)?? arctanrz(k)??Tv(k)为测量噪声，他也是高斯型白色随机向量序列，而且

E?v(k)??r1?02?1，Ev(k)vT(k)?R1

对于R1，其定义为

??R1(k)?D?1(k)xD?T(k)

其中，x?0.1I2

?r2(k)?r2(k)?r2(k)xyzD(k)???0???

222rx(k)?ry(k)?rz(k)??

显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。仿真结果为：

0error of positionreal ekf

error of position650ekf6001000 550500450900800400350300700600150040001000 5000-1000100020003000250200150 050100150200250300350400 轨迹跟踪图 位置误差

error of velocity180ekf17016015014013012011010090 0

ekferror of acceleration181614121086420 05010015020025030035040050100150200250300350400 速度误差 加速度误差

寻的制导matlab仿真程序

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 程序说明：目标跟踪程序，实现运动弹头对运动物体的三维跟踪，主函数 % 状态方程： x(t)=Ax(t-1)+Bu(t-1)+w(t） % 参考资料：《寻的导弹新型导引》第5.5和5.6节中仿真参数设置

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function main

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

delta_t=0.01; % 测量周期,采样周期

longa=1; % 机动时间常数的倒数，即机动频率 T=3.7/delta_t;% 时间长度3.7秒钟，一共采样T=370次

F=[eye(3),delta_t*eye(3),(exp(-1*longa*delta_t)+longa*delta_t-1)/longa^2*eye(3); zeros(3),eye(3),(1-exp(-1*longa*delta_t))/longa*eye(3);

zeros(3),zeros(3),exp(-1*longa*delta_t)*eye(3)]; % 状态转移矩阵fai G=[-1*0.5*delta_t^2*eye(3);-1*delta_t*eye(3);zeros(3)]; % 控制量驱动矩阵gama

…… ……

五、UKF在六维CA目标跟踪模型中的应用例子：

一、仿真问题描述

考虑一个在二维平面x-y内运动的质点M，其在某一时刻k的位置、速度和加速度可用矢量x(k)?[xk,yk,xk,yk,xk,yk]表示。假设M在水平方向（x）作近似匀加速直线运动，垂直方向（y）上亦作近似匀加速直线运动。两方向上运动具有加性系统噪声w(k)，则在笛卡尔坐标系下该质点的运动状态方程为

x(k?1)?fk(x(k))?w(k)?Fkx(k)?w(k)

T其中

??1???0Fk??

?0?0??0??0

0t

t20

2

0t010

?0??2t?2??0? t??0??1?

10t010001000000

假设一坐标位置为（0,0）的雷达对M进行测距rk和测角?k，实际测量中雷达具有加性测量噪声v(k)，则在传感器极坐标系下，观测方程为

?x2?y2?v(k)?kr?rk?vr(k)??k?z(k)?hk(x(k))?v(k)???????1yk??v(k)tan?v?(k)? k????xk??显然在笛卡尔坐标系下，该模型运动观测方程为非线性的。我们根据雷达测

量值使用UKF算法对目标进行跟踪，并与EKF算法结果进行比较。

三、实验仿真与结果分析

?1?0??0Qk??假设设系统噪声w(k)具有协方差阵?0?0???001000000000.01200?0??0??v(k)具有0?，0??0.012??00.120000.12000000?52协方差阵Rk???00?，二者不相关。观测次数N=50，采样时间为t=0.5。初始2?0.01?Tx(0)?[1000,5000,10,50,2,?4]状态。则生成的运动轨迹如图1所示。

5350实际值测量值ukf估计值 5300525052005150510050505000 1000110012001300140015001600170018001900

轨迹跟踪图