所示。
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
轨迹跟踪图