机动目标跟踪与反跟踪（附录）

参赛密码 （由组委会填写）

第十一届华为杯全国研究生数学建模竞赛

学 校 参赛队号

空军工程大学 90045035 1.唐 茂

队员姓名

2.史 密 3.李世杰

参赛密码 （由组委会填写）

第十一届华为杯全国研究生数学建模竞赛

题 目 机动目标的跟踪与反跟踪

摘 要：

为解决机动目标的跟踪与反跟踪问题，本文综合运用航迹拟合、数据关联、滤波原理，建立了目标的运动模型，并基于自适应卡尔曼滤波原理建立了目标跟踪模型。运用所建立的模型进行仿真，估计和预测了目标机动情况，提出了跟踪与反跟踪策略。

针对问题一，首先将雷达量测数据按照坐标转换公式统一到地心地固坐标系中，采用多项式拟合方法估计目标初始状态；其次综合考虑，确定建立Singer模型来跟踪目标运动状况；最后设计了卡尔曼滤波器估计目标的状态信息，得到目标的航迹。

针对问题二，进行降维处理简化模型，通过数据关联算法分离测量信息，确定测量数据属于哪个目标；依据两个目标的测量数据分别进行卡尔曼滤波得到两个目标各自的航迹图；分析了当雷达一段时间只有一个回波点迹时，怎样使得航迹不丢失。

针对问题三，依据Data3中的数据描点做出点迹图；由于目标运动轨迹较为简单，采用多项式拟合得到目标的拟合航迹一。再依据题目要求，采用问题一所建立的跟踪模型进行处理，得到拟合航迹二。分别对两条航迹进行二次差分得到目标加速度随时间变化规律，并作对比。

针对问题四，依据最小二乘估计，对问题三得到的拟合航迹一做预测；目标着落点即预测航迹与地平面的交点，联立方程求此着落点在以雷达为原点的站心切平面坐标系中坐标。最后对比分析两种预测算法的复杂度，发现复杂度随预测步数指数增加。

针对问题五，建立两种机动动作模型；分析了问题一的跟踪模型对采用以上机动动作的目标的跟踪能力；确定了跟踪策略；采用交互式多模型算法对问题一建立的模型进行完善；调整了跟踪策略。

关键词：目标跟踪；数据关联；卡尔曼滤波；Singer模型；数据拟合；实时预测

- 1 -

一、问题重述

目标跟踪[1]是为了维持对目标当前状态的估计，同时也是对传感器接收到的量测进行处理的过程（多源信息融合），它在军事和民用领域都已经得到了广泛的应用。同时，被跟踪目标为了反跟踪往往会进行机动或释放干扰，这对目标跟踪技术提出了更高的要求。

本题介绍了机动目标跟踪的难点以及目标跟踪处理流程，给定了三组机动目标的测量数据以及雷达坐标和测量误差。其中Data1给定了多个雷达站在不完全相同时刻获得的单个机动目标的测量数据；Data2给定了某个雷达站获得的两个机动目标的测量数据；Data3给定了某个雷达站获得的空间目标的测量数据。

根据已知内容，需要解决的问题如下： 问题一：

1）根据Data1.txt中的数据，分析目标机动发生的时间范围，统计目标加速度的大小和方向。

2）建立对该目标的跟踪模型，利用多个雷达的测量数据估计出目标的航迹。 问题二：

1）根据Data2.txt中的数据，完成各目标的数据关联，形成相应的航迹，并阐明所采用或制定的准则。

2）通过处理保证若出现雷达一段时间只有一个回波点迹的状况，可使得航迹不丢失。 问题三：

1)根据Data3.txt中的数据，分析空间目标的机动变化规律(目标加速度随时间变化)。

2)若采用第1问的跟踪模型进行处理，结果会有哪些变化？ 问题四：

对第3问的目标轨迹进行实时预测，估计该目标的着落点的坐标，给出详细结果，并分析算法复杂度。 问题五：

1）Data2.txt数据中的两个目标已被雷达锁定跟踪。分析该目标应采用怎样的有利于逃逸的策略与方案来应对之前所建立的跟踪模型。

2）分析为了保持对目标的跟踪，跟踪策略又应该如何相应地变换 。

- 2 -

二、模型假设

1，假设雷达均处在正常工作状态。

2，假设电磁波在传播过程中符合几何光学定律。 3, 假设目标的机动符合现有的技术水平。

3，假设目标在飞行过程中不会发生坠毁等突发状况。 4，忽略多径效应对测量结果的影响。

三、基本符号说明

r HA B L x y z XkX?kZk 俯仰角 方位角 大地纬度 大地经度 x轴坐标 y轴坐标 z轴坐标 当前时刻状态 状态估计 量测量

- 3 -

Kk 滤波增益 P 均方误差 ? 误差向量 ? 机动频率 d(k) 统计距离 ? 跟踪门限 A 多项式系数矩阵? 飞行坡度 Ptij 状态转移矩阵 ? 残差 ? 雷达波束宽 目标到雷达距离 四、问题一模型的建立与求解

4.1 问题一分析

Data1给定了多个雷达站在不完全相同时刻测量获得的单个机动目标的距离、方位、俯仰和时间数据信息。由于雷达量测值为目标距离方位和俯仰，为使得观测数据更直观，并简化下文所建立模型的状态方程和量测方程，需要进行坐标转换以统一坐标系：

首先将距离，方位和仰角的极坐标信息转换到站心切平面坐标系中。题目中站心切平面坐标系为：原点设为雷达中心，传感器中心点与当地纬度切线方向指向东为x轴，传感器中心点与当地经度切线方向指向北为y轴，地心与传感器中心连线指向天向的为z轴，目标方位指北向顺时针夹角（从y轴正向向x轴正向的夹角，范围为0~360°），目标俯仰指传感器中心点与目标连线和地平面的夹角，据此，极坐标转换为站心坐标转换公式[2]为：

?x?rcosHsinA??y?rcosHsinA (4.1) ?z?rsinH?式中(x,y,z)是目标在站心切平面坐标系中的坐标，r为目标量测距离，H为目标俯仰角，A为目标方位角。

依据坐标转换得到的数据，在Matlab中编程（见附件），直接呈现该机动目标的量测数据，如下图所示：

20001000z0-10005.53.8x 10453.44.53y42.8x3.23.6x 104图4.1 雷达一量测值

- 4 -

22式中?m，?是在区间[t,t??]内决定目标机动特征的待定参数；?m是目标的加速度

方差，?是机动时间的倒数，即机动频率，通常?的经验取值范围是：目标是飞机慢速转弯，1取60s，对于逃避机动取20s。

?Singer对应的离散时间状态方程为：

X(k?1)?F(k)X(k)?V(k) (4.22)

??T?(?T?1?e)?1T??2?????T(1?e)? (4.23) ??01?????T?00?e????F?eAT?q112?其离散时间过程噪声V具有协方差Q?2??m?q21??q31q12q22q32q13?q23??，且Q为对称阵。 q33??其中：

?12?3T2?2?T22??Tq?[1?e?2?T??2?T?4?Te]?1152?3??q?1[e?2?T?1?2e??T?2?Te??T?2?T??2T2]?122?4??q?1[1?e?2?T?2?Te??T]?132?3 (4.24) ??q?1[4e??T?3?e?2?T?2?T]?222?3??q23?12[e?2?T?1?2e??T]2???1[1?e?2?T]?q33?2??4.2.3仿真结果

在确定了滤波初始值和运动模型后，即可估计目标的运动轨迹。滤波模型中的核心过程[9]如下：

for i=1:l-1

Xkk1(:,i)=Fk*Xkk(:,i); %一步预测

Pkk1=Fk*Pkk*Fk'; %一步预测均方误差

Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R); %滤波增益

Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zkcl(:,i)-Hk*Xkk1(:,i)); %状态估计 Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; %估计均方误差 end

10 - -

滤波结果如下图所示，

x 103.154150003.1100003.0550003150001000050000-50000

图4.8 滤波结果

由此可知目标在被跟踪期间发生的较为明显的机动时间，具体信息由表1给出

表 1 机动发生时间表 机动序号 发生时间(s) 1 36724.4—36744.4 2 36961.4—37199.4 4.3跟踪模型稳定性研究 ??E?X?,P?Var[X]时，滤波器的估计从开始就是无偏的，且估上文指出，当X0000计均方误差是最小的。但是在本题的具体实践中，很难得到初始状态的验前统计值。如

?和P各自都逐渐不受其初值的影响，则滤波器是稳定的。果滤波器随时间的增加，Xkk下面分析本题中所建立的卡尔曼滤波器的稳定性。

滤波稳定条件最早由卡尔曼提出，参考文献[10]提出了离散系统滤波稳定的充分条件。

离散系统状态方程和量测方程为:

Xk?Fk,k?1Xk?1?Wk (4.25) Zk?HkXk?Vk (4.26)

式中

?E[Wk]?0,E[WkWjT]?Qk?kj?T?E[Vk]?0,E[VkVj]?Rk?kj (4.27)

T?E[WV]?0kj? 11 - -

如果系统一致完全随机可控和一致完全随机可观测，且Qk 和Rk正定，则卡尔曼滤波器是一直逐渐稳定的。 完全随机可控指：

?(k,k?N?1)?i?k?N?1?kkFk,iTQiT?1FkT,i?0 (4.27)

完全随机可观测指：

?(k,k?N?1)?i?k?N?1?Fi,kTHiTRi?1HiFiT,k?0 (4.28)

结合本题情况，从物理意义可见系统满足随机可控和随机可观测的条件。从Kk的过程来看：

??Kk?Pkk?1??????Pkk?1(1,1)???1??Pkk?1(1,1)?R?Pkk?1(1,1)?R??Pkk?1(2,1)???? (4.29) 0???P(1,1)?R???kk?10??P(3,1)???kk?1?P(1,1)?R??kk?1?可见，Kk始终有值，使每一步计算都能利用量测中的最新信息，修正旧估计，得到新的实时估计。一般在P随估计过程的进行，Pk?1是逐渐下降的，0去较大值得情况下，下降程度去Q和R有关。而Pkk?1却比Pk?1大，增大的值与Q有关。因此当下降值与增大值相当时，Kk趋于稳定值，滤波呈稳定状态。

实际在问题求算过程中，因为观测值有限，为了防止滤波稳定时间超过量测时间，

所以本文采用拟合算法选取目标状态初始值是有必要的和可行的。

12 - -

五、问题二模型的建立与求解

5.1问题二分析

Data2给定了雷达获得的两个机动目标的测量数据，为了得到两个机动目标各自的航迹，首先要判断信息属于哪个目标，即进行数据关联[11]。

本文先根据Data2中的数据直接得到目标点迹来粗略观测目标的轨迹，通过坐标变换得到目标在站心切平面坐标系中的坐标，描点可得下图：

5500500045004000350030002500200015001.510.50x 104-0.5-1-1.5123456789x 104

图5.1 观测数据描点图

由于目标的不规则运动以及误差的影响，得到的点迹图杂乱无章，很难提取有效信息。因此我们进行了降维处理，得到目标运动轨迹在水平面的投影如下图：

1.5x 10410.50-0.5-1-1.5-1012345678x 1094

图5.2 目标水平面运动轨迹

从图中可以清楚的看到两条轨迹，即目标飞行包线在水平面的投影。再以这两条粗略的轨迹为基础，依据5.2中的数据关联算法分别得到两条不同的投影，从而得到两个目标的航迹。

5.2问题二模型的建立与求解

13 - -

1）主成分线性回归得到初始航迹投影

从已给数据可知雷达的扫描周期较小（1s左右），且目标据雷达距离相对较远。可以假定在一小段时间内雷达扫描两个目标的先后顺序没有发生变化，由于目标的机动要受到诸多限制，所以这种假定可以认为是合理的。考查前200个点，分别取奇数点和偶数点得到以下两图，把它们作为两个目标的初始航迹投影粗略值：

1200020001000008000-20006000-40004000-60002000-80000-10000-20007.67.77.87.988.18.28.38.48.5x 104-120007.67.77.87.988.18.28.38.48.5x 104

图5.3 初始航迹估计

通过主成分线性回归[12]得：

120002000100000-20008000-40006000-60004000-80002000-1000007.67.77.87.988.18.28.38.48.5x 104-120007.67.77.87.988.18.28.38.48.5x 104

图5.4 初始航迹

由此得到粗略的初始航迹，可以为下面的数据关联算法和卡尔曼滤波提供所需的初始值。

2)数据关联算法得到两个目标的航迹投影 计算目标的预测位置和有效回波之间的距离：

??1????kk?1??d2?z?k????zk?zkk?1??????S?k???z?k??z?

????k?S?1?k???k? (5.1)

其中??k?为卡尔曼滤波的残差，S?k?表示v?k?的协方差，d?k?为定义的统计距离。当满足d?k???时，则保留该点击，不满足的则剔除。其中?为跟踪门限（采用矩形跟踪门），如此可以得到与航迹配对的所有可能的点迹。若此时只有一个点迹满足要求,就取该点迹为目标的新点迹。反之，当跟踪门内的点迹大于1个时,则将d?k?最小的点迹作为新的点迹。

14 - -

卡尔曼滤波跟踪轨迹图5x 10432z10-143x 105021y0-15-10x-5x 105

图6.6 卡尔曼滤波跟踪轨迹图

上方的轨迹为采用第一问的跟踪模型得到的结果，下方的轨迹为采用拟合算法得到的轨迹图。

4）在第一问跟踪模型这一前提下，分析目标在运动过程中x、y、z坐标值相对时间t的变化情况，对其进行两次差分得到目标在三个方向的加速度变化情况结果如下图：

1086420-2-4-6-8-100100200300400500600

图6.7 x轴方向加速度变化情况

20 - -

1086420-2-4-6-8-100100200300400500600

图6.8 y轴方向加速度变化情况

1086420-2-4-6-8-100100200300400500600

图6.9 z轴方向加速度变化情况

21 - -

七、问题四的求解

4.1多项式拟合预测原理

以x轴方向为例，为得到x轴方向上x坐标与量测时间的关系，多项式拟合即根据已知的k个量测点，i时刻量测数据位X(i)，i=1,2，..K，求出一条m次多项式曲线P(t)，使其在i处的取值尽量接近X(i)。设所求多项式

P(t)?a0?a1t?a2t2?

令

?amtm (7.1)

?1??t1?t12???tm?11t22t2mt21t32t3mt31??tk?tk2???m?tk?T?a0??X(t1)??R1???????aX(t)12?????R2??a2???X(t3)???R3? (7.2) ?????????????a??X(t)??R?k??m???k?k求解过程就是选取适合的多项式系数，使得???Rn2最小。

n?1上式的解可以用矩阵XA?Y的解来表示，其中

?t1m?mtX??2???tm?kt11??am??X(t1)?????X(t)?at21?m?12??，Y??，A?? (7.3) ??????????aX(t)tk1?0??k???记XTX?W，则

A?W?1XTY (7.4)

同理可得y轴和z轴上的多项式，三个多项式组合可以得到目标三维的参数方程。

4.2着落点求解

为估计目标的着落点，需要联立大地平面方程和目标运动轨迹方程，方程的解即是目标的预测着落点。

问题三中目标运动参数方程为

x?2.545645186807171?10?7t4?5.625067878491474?10?4t3?0.824027804137516t2+1.790771922754607?103t?1.336404636158222?106

22 - -

y?5.573755284745952?10?7t4?4.971552451791386?10?4t3?0.087774077360674t2?3.485884424972124?102t+2.966491766494982?105z??5.090952285965095?10?7t4+4.687861298581880?10?4t3?4.376396324729971t2+2.398606602752963?103t+3.918336686398332?103

当z=0时，求得x??1.4214?105， y?3.9925?104

54着落点在站心切平面坐标系下为[?1.4214?103.9925?100]

4.3方法复杂度分析

由多项式拟合原理可以看到，已知k个量测点的情况下，要预测k+1时刻的点需要之前所有k个数据做最小二乘。而最小二乘求解的运算量与矩阵阶数的三次方成正比。当预测第k+2时刻时，算法需要把预测的第k+1个量作为量测值，故需要之前k+1个数据参与运算。分析得知该方法的运算复杂度是随预测点数的增加而成三次方的速度增加。

若采用问题一所建立的卡尔曼滤波模型进行预测，当量测值用完时，为了预测下一点的状态，把下一时刻的一步估计值作为量测值处理。这样做虽然引入了一定的误差，但可以使得卡尔曼能继续向下递推，从而得到期望的预测值。

卡尔曼的特点是采用了地推算法，不同时刻的量测值不必储存下来，而是经实时处理提炼成被估计状态的信息，随着滤波步数的增加，提取出的信息浓度逐渐增加[15]。对于对状态的预测，算法的复杂度不会随预测步数的增加而增加。

23 - -

八、问题五模型的建立与求解

8.1问题五分析

当目标发现自身已经被雷达锁定后，可以采取一定的机动动作来逃避雷达的跟踪。为了确定目标的逃逸策略，我们建立了两种飞机典型机动动作模型[16]，即盘旋机动模型和蛇形机动模型。以问题一中提出的跟踪模型为基础分析了这两种机动动作对雷达跟踪能力的影响，从而确定目标反跟踪的策略。

为了保持对目标的跟踪，通过交互式多模算法对问题一中的模型进行完善，并仿真分析新模型的跟踪效果，调整跟踪策略。 8.2反跟踪模型的建立与求解

1）建立目标盘旋机动模型

当目标进行盘旋机动时，目标的盘旋运动方程应满足：

?L?D? (8.1) ?Lcos??mg?Lsin??mV2/rpp?式中:L为垂直于迎面空气流的升力;D为与推力相反的阻力;?为飞行坡度;m为飞机质量;Vp为目标盘旋机动速度;rp为目标盘旋半径;g为重力加速度。根据目标盘旋运动方程，可以得到目标盘旋半径rp为：

rp?mVp2/Lsin? (8.2)

盘旋是经典的机动方式之一，如果将目标假设为飞机，做动作设计时还要考虑实际飞行中飞行员所能承受的最大载荷n。飞机作盘旋时，比飞机平飞时的速度扩大n倍，比平飞时所需的推力扩大n倍。

2）建立目标蛇形机动模型

目标在进行蛇形机动时，其运动方程满足如下方程组：

m?x???t? (8.3) ??tnr?cost??y???e其中?和nr为目标机动参数。

3）采用问题一中建立的跟踪模型分别对进行盘旋机动和蛇形机动的目标进行跟踪，

得到跟踪结果如下图：

24 - -

XXa2=(1-exp(-T/a))*a; XXa3=exp(-T/a); a=20000;

YYa1=(T/a-1+exp(-T/a))*a^2; YYa2=(1-exp(-T/a))*a; YYa3=exp(-T/a); a=20;

ZZa1=(T/a-1+exp(-T/a))*a^2; ZZa2=(1-exp(-T/a))*a; ZZa3=exp(-T/a); Hk=[1 0 0 0 0 0 0 0 0; 0 0 0 1 0 0 0 0 0; 0 0 0 0 0 0 1 0 0]; Fk=[1 T XXa1 0 0 0 0 0 0; 0 1 XXa2 0 0 0 0 0 0; 0 0 XXa3 0 0 0 0 0 0; 0 0 0 1 T YYa1 0 0 0 ; 0 0 0 0 1 YYa2 0 0 0; 0 0 0 0 0 YYa3 0 0 0; 0 0 0 0 0 0 1 T ZZa1; 0 0 0 0 0 0 0 1 ZZa2; 0 0 0 0 0 0 0 0 ZZa3];

x=Zk(1,1);y=Zk(2,1);z=Zk(3,1);l=size(Zk,2)+1;

diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2); yyy=diffy(1);

diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); xxx=diffx(1);

diffz=diff(Zk(3,:),1);zz=diffz(1);diffz=diff(Zk(3,:),2); zzz=diffz(1);

P0=[4e6 0 0 0 0 0 0 0 0; 0 6e7 0 0 0 0 0 0 0; 0 0 4e5 0 0 0 0 0 0; 0 0 0 6e7 0 0 0 0 0; 0 0 0 0 4e9 0 0 0 0; 0 0 0 0 0 3e5 0 0 0; 0 0 0 0 0 0 6e3 0 0; 0 0 0 0 0 0 0 3e4 0; 0 0 0 0 0 0 0 0 8e3 ];

X0=[x xx xxx y yy yyy z zz zzz]';R=eye(3)*sqrt(40/3); Xkk1=zeros(9,l); Pkk=P0;

Xkk=zeros(9,l);Xkk(:,1)=X0; for i=1:l-1

Xkk1(:,i)=Fk*Xkk(:,i); Pkk1=Fk*Pkk*Fk';

35 - -

Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R);

Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i)); Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(1)

plot3(Zk(1,:),Zk(2,:),Zk(3,:),'b:'); hold on

plot3(Xkk(1,:),Xkk(4,:),Xkk(7,:),'r.-'); xlabel('x');ylabel('y');zlabel('z'); hold off grid on

% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%二维singer model%%%%%%%%%%%%%%%%%%%%%%%% T=0.5;

Zk=[XX1 YY1]'; a=2e1;

XXa1=(T/a-1+exp(-T/a))*a^2; XXa2=(1-exp(-T/a))*a; XXa3=exp(-T/a); a=9e6;

YYa1=(T/a-1+exp(-T/a))*a^2; YYa2=(1-exp(-T/a))*a; YYa3=exp(-T/a); Hk=[1 0 0 0 0 0 ; 0 0 0 1 0 0 ;]; Fk=[1 T XXa1 0 0 0 ; 0 1 XXa2 0 0 0 ; 0 0 XXa3 0 0 0 ; 0 0 0 1 T YYa1 ; 0 0 0 0 1 YYa2 ; 0 0 0 0 0 YYa3 ;];

x=Zk(1,1);y=Zk(2,1);l=size(Zk,2)+1;

diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2); yyy=diffy(1);

diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); xxx=diffx(1); P0=[50 1 2 3 4 5; 2 60 2 3 4 5; 1 2 70 3 2 3; 1 2 3 76 2 3; 1 2 1 3 76 6; 2 1 3 2 4 89]; X0=[x xx xxx y yy yyy]';

36 - -

R=eye(2,2)*sqrt(40/2);

Xkk1=zeros(6,l);Xkk=zeros(6,l);Xkk(:,1)=X0; Pkk=P0;

for i=1:l-1

Xkk1(:,i)=Fk*Xkk(:,i); Pkk1=Fk*Pkk*Fk';

Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R);

Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i)); Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(5)

plot(Zk(1,:),Zk(2,:),'b:'); hold on

plot(Xkk(1,:),Xkk(4,:),'r.-'); xlabel('x');ylabel('y'); hold off grid on

% %%%%%%%%%%%%%%%%%%%%%%%%ct model%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

T=1;

Zk=[x3 y3]'; omega=2*pi/130; Hk=[1 0 0 0 ; 0 0 1 0;];

Fk=[1 sin(omega)/omega 0 -(1-cos(omega))/omega ; 0 cos(omega) 0 -sin(omega) ; 0 (1-cos(omega))/omega 1 sin(omega)/omega ; 0 sin(omega) 0 cos(omega) ;]; x=Zk(1,1);y=Zk(2,1);l=size(Zk,2)+1;

diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2); diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); X0=[x xx y yy ]';Xkk=zeros(4,l);Xkk(:,1)=X0; P0=[40 12 3 1; 12 50 3 4; 2 21 50 4; 2 5 1 50];

R=eye(2,2)*sqrt(40/2); Xkk1=zeros(4,l);l=size(Zk,2); Pkk=P0;

37 - -

for i=1:l-1

Xkk1(:,i)=Fk*Xkk(:,i); Pkk1=Fk*Pkk*Fk';

Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R);

Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i)); Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(4)

plot(Zk(1,:),Zk(2,:),'b:'); hold on

plot(Xkk(1,1:l-1),Xkk(3,1:l-1),'r.'); xlabel('x');ylabel('y'); hold off grid on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%当前统计模型%%%%%%%%%%%%%%%%%%%%%%%%%%%%% T=1;

Zk=[x3 y3]'; a=2e1;

Gk1=[a*(-T+T^2/a/2+a*(1-exp(-T/a)));T-a*(1-exp(-T/a));1-exp(-T/a)]; XXa1=(T/a-1+exp(-T/a))*a^2; XXa2=(1-exp(-T/a))*a; XXa3=exp(-T/a); a=9e6;

Gk2=[a*(-T+T^2/a/2+a*(1-exp(-T/a)));T-a*(1-exp(-T/a));1-exp(-T/a)]; YYa1=(T/a-1+exp(-T/a))*a^2; YYa2=(1-exp(-T/a))*a; YYa3=exp(-T/a); Hk=[1 0 0 0 0 0 ; 0 0 0 1 0 0 ;]; Fk=[1 T XXa1 0 0 0 ; 0 1 XXa2 0 0 0 ; 0 0 XXa3 0 0 0 ; 0 0 0 1 T YYa1 ; 0 0 0 0 1 YYa2 ; 0 0 0 0 0 YYa3 ;];

x=Zk(1,1);y=Zk(2,1);l=size(Zk,2)+1;

diffy=diff(Zk(2,:),1);yy=diffy(1);diffy=diff(Zk(2,:),2);

38 - -

yyy=diffy(1);

diffx=diff(Zk(1,:),1);xx=diffx(1);diffx=diff(Zk(1,:),2); xxx=diffx(1); P0=[50 1 2 3 4 5; 2 60 2 3 4 5; 1 2 70 3 2 3; 1 2 3 76 2 3; 1 2 1 3 76 6; 2 1 3 2 4 89];

X0=[x xx xxx y yy yyy]'; R=eye(2,2)*sqrt(40/2);

Xkk1=zeros(6,l);Xkk=zeros(6,l);Xkk(:,1)=X0; Pkk=P0;

Qk=0*[1/20 1/8 1/6 0 0 0; 1/8 1/3 1/2 0 0 0; 1/6 1/2 1 0 0 0; 0 0 0 1/20 1/8 1/6; 0 0 0 1/8 1/3 1/2; 0 0 0 1/6 1/2 1]; for i=1:l-1

Xkk1(:,i)=Fk*Xkk(:,i) +cat(1,Gk1*Xkk1(3,i),Gk2*Xkk1(6,i)); Pkk1=Fk*Pkk*Fk'+Qk;

Kk=Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R);

Xkk(:,i+1)=Xkk1(:,i)+Kk*(Zk(:,i)-Hk*Xkk1(:,i))+cat(1,Gk1*Xkk1(3,i),Gk2*Xkk1(6,i));

Pkk=Pkk1-Pkk1*Hk'*inv(Hk*Pkk1*Hk'+R)*Hk*Pkk1; end figure(3)

plot(Zk(1,:),Zk(2,:),'b:'); hold on

plot(Xkk(1,:),Xkk(4,:),'r.-'); xlabel('x');ylabel('y'); hold off grid on

39 - -

本文来源：https://www.bwwdw.com/article/u1ig.html