扩展卡尔曼滤波EKF—目标跟踪中的应用(算法部分)
原创不易，路过的各位大佬请点个赞
机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313
仿真部分见博客：
扩展卡尔曼滤波EKF—目标跟踪中的应用(仿真部分)
/weixin_44044161/article/details/115329181
作者:823618313@
备注：
扩展卡尔曼滤波算法；
目标跟踪matlab仿真实现;
Case: 二维目标跟踪情况和三维目标跟踪情况
代码下载地址如下（分别为二维情形和三维情形）
扩展卡尔曼滤波 机动目标跟踪 EKF
/download/weixin_44044161/85401461
扩展卡尔曼滤波EKF匀速圆周运动CT
/download/weixin_44044161/85401885
EKF仿真代码：二维目标跟踪
/download/weixin_44044161/85123812
EKF仿真代码：三维目标跟踪
/download/weixin_44044161/85123744
扩展卡尔曼滤波—及其在目标跟踪中的应用
- 扩展卡尔曼滤波EKF—目标跟踪中的应用(算法部分)
- 一、带加性噪声的扩展卡尔曼滤波算法
- 1.1 问题描述（离散时间非线性系统描述）
- 1.2 扩展卡尔曼滤波器（EKF）
- 二、带非加性噪声的扩展卡尔曼滤波算法
- 2.1 问题描述（离散时间非线性系统描述）
- 2.2 带非加性噪声的扩展卡尔曼滤波器（EKF）
- 三、仿真实验
- 3.1 仿真场景（三维）
- 3.2 仿真场景（二维）
- 3.3 二维EKF跟踪仿真结果
- 四、二维EKF跟踪参考代码
- 4.1 主函数
一、带加性噪声的扩展卡尔曼滤波算法
1.1 问题描述（离散时间非线性系统描述）
考虑带加性噪声的一般非线性系统模型，
x
k
=
f
(
x
k
−
1
)
+
w
k
−
1
z
k
=
h
(
x
k
)
+
v
k
(1)
x_k=f(x_{k-1}) +w_{k-1} \\ z_k=h(x_k)+v_k \tag{1}
xk=f(xk−1)+wk−1zk=h(xk)+vk(1)
其中
x
k
x_k
xk为
k
k
k时刻的目标状态向量。
z
k
z_k
zk为
k
k
k时刻量测向量（传感器数据）。这里不考虑控制器
u
k
u_k
uk。
w
k
{w_k}
wk和
v
k
{v_k}
vk分别是过程噪声序列和量测噪声序列，并假设
w
k
w_k
wk和
v
k
v_k
vk为零均值高斯白噪声，其方差分别为
Q
k
Q_k
Qk和
R
k
R_k
Rk的高斯白噪声，即
w
k
∼
(
0
,
Q
k
)
w_k\sim(0,Q_k)
wk∼(0,Qk),
v
k
∼
(
0
,
R
k
)
v_k\sim(0,R_k)
vk∼(0,Rk)，且满足如下关系(线性高斯假设)为：
E
[
w
i
v
j
′
]
=
0
E
[
w
i
w
j
′
]
=
0
i
≠
j
E
[
v
i
v
j
′
]
=
0
i
≠
j
\begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned}
E[wivj′]E[wiwj′]E[vivj′]=0=0i=j=0i=j
1.2 扩展卡尔曼滤波器（EKF）
1.） 初始化
给定
k
−
1
k-1
k−1时刻的状态估计和协方差矩阵
x
^
k
−
1
∣
k
−
1
,
P
k
−
1
∣
k
−
1
,
Q
k
−
1
,
R
k
−
1
\hat{x}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1}
x^k−1∣k−1,Pk−1∣k−1,Qk−1,Rk−1
当为
0
0
0时刻时，滤波器最优初始化为
x
0
∼
(
x
ˉ
0
,
P
0
)
,
Q
0
,
R
0
x_0\sim(\bar{x}_0, P_0),Q_{0},R_{0}
x0∼(xˉ0,P0),Q0,R0
即
x
^
0
∣
0
=
x
ˉ
0
P
0
∣
0
=
P
0
\hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0
x^0∣0=xˉ0P0∣0=P0
2. ) 状态预测
2.1 计算非线性系统方程的雅可比矩阵
F
k
−
1
=
∂
f
(
x
k
−
1
)
∂
x
k
−
1
∣
x
k
−
1
=
x
^
k
−
1
∣
k
−
1
F_{k-1}=\frac{\partial f\left(x_{k-1}\right)}{\partial x_{k-1}}\Big|_{{x_{k-1}=\hat{x}_{k-1|k-1}}}
Fk−1=∂xk−1∂f(xk−1)∣
∣xk−1=x^k−1∣k−1
2.2 状态一步预测及预测误差协方差阵为
x
^
k
∣
k
−
1
=
E
[
x
k
∣
Z
k
−
1
]
≈
E
[
f
k
−
1
(
x
^
k
−
1
∣
k
−
1
,
0
)
+
F
k
−
1
x
~
k
−
1
∣
k
−
1
+
w
k
−
1
∣
Z
k
−
1
]
=
f
k
−
1
(
x
^
k
−
1
∣
k
−
1
)
P
k
∣
k
−
1
=
cov
(
x
~
k
∣
k
−
1
)
=
F
k
−
1
P
k
−
1
∣
k
−
1
F
k
−
1
′
+
Q
k
−
1
(2)
\textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k-1}&=E\left[ x_k\mid Z^{k-1}\right ]\\&\approx E\left[f_{k-1}\left(\hat{x}_{k-1|k-1},0\right)+F_{k-1}\widetilde{x}_{k-1|k-1}+w_{k-1} \mid Z^{k-1}\right]\\&=f_{k-1}\left(\hat{x}_{k-1|k-1}\right)\\ P_{k\mid k-1}&=\text{cov}\left(\widetilde{x}_{k\mid k-1}\right)= F_{k-1}P_{k-1|k-1}F_{k-1}'+Q_{k-1} \end{aligned}} \tag{2}
x^k∣k−1Pk∣k−1=E[xk∣Zk−1]≈E[fk−1(x^k−1∣k−1,0)+Fk−1x
k−1∣k−1+wk−1∣Zk−1]=fk−1(x^k−1∣k−1)=cov(x
k∣k−1)=Fk−1Pk−1∣k−1Fk−1′+Qk−1(2)
其中系统状态方程在
x
^
k
−
1
∣
k
−
1
\hat{x}_{k-1|k-1}
x^k−1∣k−1的泰勒级数展开为（取一阶）
x
k
=
f
k
−
1
(
x
k
−
1
)
+
w
k
−
1
≈
f
k
−
1
(
x
^
k
−
1
∣
k
−
1
)
+
F
k
−
1
x
~
k
−
1
∣
k
−
1
+
w
k
−
1
\begin{aligned} x_k&=f_{k-1}\left(x_{k-1} \right) + w_{k-1}\\ &\approx f_{k-1}\left(\hat{x}_{k-1|k-1}\right)+F_{k-1}\widetilde{x}_{k-1|k-1}+w_{k-1} \end{aligned}
xk=fk−1(xk−1)+wk−1≈fk−1(x^k−1∣k−1)+Fk−1x
k−1∣k−1+wk−1
预测估计误差为
x
~
k
∣
k
−
1
=
x
k
−
x
^
k
∣
k
−
1
≈
F
k
−
1
x
~
k
−
1
∣
k
−
1
+
w
k
−
1
\tilde{x}_{k\mid k-1}=x_k-\hat{x}_{k|k-1}\approx F_{k-1}\widetilde{x}_{k-1|k-1}+ w_{k-1}
x~k∣k−1=xk−x^k∣k−1≈Fk−1x
k−1∣k−1+wk−1
3.) 量测预测
3.1 计算量测系统方程的雅可比矩阵
H
k
=
∂
h
(
x
k
)
∂
x
k
∣
x
k
=
x
^
k
∣
k
−
1
H_{k}=\frac{\partial h\left(x_{k}\right)}{\partial x_{k}}\Big|_{{x_{k}=\hat{x}_{k|k-1}}}\
Hk=∂xk∂h(xk)∣
∣xk=x^k∣k−1
3.2 量测一步预测、新息协方差、互协方差为
z
^
k
∣
k
−
1
=
E
[
z
k
∣
Z
k
−
1
]
=
h
(
x
^
k
∣
k
−
1
)
S
k
=
cov
(
z
~
k
∣
k
−
1
)
=
H
k
P
k
∣
k
−
1
H
k
′
+
R
k
C
k
=
cov
(
x
~
k
∣
k
−
1
,
z
~
k
∣
k
−
1
)
=
P
k
∣
k
−
1
H
k
′
(3)
\textcolor{#FF0000}{ \begin{aligned} \hat{z}_{k|k-1}&=E\left[z_k\mid Z^{k-1}\right]= h\left(\hat{x}_{k|k-1}\right)\\ S_k&=\text{cov}\left(\widetilde{z}_{k\mid k-1}\right)= H_{k}P_{k|k-1}H_{k}'+ R_{k}\\ C_k&=\text{cov}\left(\widetilde{x}_{k\mid k-1},\widetilde{z}_{k\mid k-1}\right)= P_{k|k-1}H_{k}' \end{aligned}} \tag{3}
z^k∣k−1SkCk=E[zk∣Zk−1]=h(x^k∣k−1)=cov(z
k∣k−1)=HkPk∣k−1Hk′+Rk=cov(x
k∣k−1,z
k∣k−1)=Pk∣k−1Hk′(3)
其中量测方程在
x
^
k
∣
k
−
1
\hat{x}_{k|k-1}
x^k∣k−1的泰勒级数展开为（取一阶）
z
k
=
h
(
x
k
)
+
v
k
≈
h
k
(
x
^
k
∣
k
−
1
)
+
H
k
x
~
k
∣
k
−
1
+
v
k
−
1
\begin{aligned} z_k&=h\left(x_{k} \right) + v_{k}\\ &\approx h_{k}\left(\hat{x}_{k|k-1}\right)+H_{k}\widetilde{x}_{k|k-1}+v_{k-1} \end{aligned}
zk=h(xk)+vk≈hk(x^k∣k−1)+Hkx
k∣k−1+vk−1
量测预测误差为
z
~
k
∣
k
−
1
=
z
k
−
z
^
k
∣
k
−
1
≈
H
k
z
~
k
∣
k
−
1
+
v
k
\tilde{z}_{k\mid k-1}=z_k-\hat{z}_{k|k-1}\approx H_{k}\widetilde{z}_{k|k-1}+ v_{k}
z~k∣k−1=zk−z^k∣k−1≈Hkz
k∣k−1+vk
4.) 状态更新
4.1 卡尔曼增益为
K
k
=
C
k
S
k
−
1
=
P
k
∣
k
−
1
H
k
′
(
H
k
P
k
∣
k
−
1
H
k
′
+
R
k
)
−
1
(4)
\textcolor{#FF0000}{ \begin{aligned} K_k&=C_kS_k^{-1}\\ &=P_{k|k-1}H_{k}'( H_{k}P_{k|k-1}H_{k}'+ R_{k})^{-1} \end{aligned}} \tag{4}
Kk=CkSk−1=Pk∣k−1Hk′(HkPk∣k−1Hk′+Rk)−1(4)
4.2 状态更新为
x
^
k
∣
k
=
E
[
x
k
∣
Z
k
]
=
x
^
k
∣
k
−
1
+
K
z
(
z
k
−
z
^
k
∣
k
−
1
)
P
k
∣
k
=
cov
(
x
~
k
∣
k
)
=
P
k
∣
k
−
1
−
K
k
S
k
K
k
′
(5)
\textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}&=E\left[ x_k\mid Z^{k}\right ]=\hat{x}_{k|k-1}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}&=\text{cov}\left(\widetilde{x}_{k\mid k}\right)=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{5}
x^k∣kPk∣k=E[xk∣Zk]=x^k∣k−1+Kz(zk−z^k∣k−1)=cov(x
k∣k)=Pk∣k−1−KkSkKk′(5)
其中估计误差为
x
~
k
∣
k
=
x
k
−
x
^
k
∣
k
\widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k}
x
k∣k=xk−x^k∣k
以上公式（2-5）及为带加性噪声非线性系统的扩展卡尔曼滤波算法。
二、带非加性噪声的扩展卡尔曼滤波算法
2.1 问题描述（离散时间非线性系统描述）
考虑带非加性噪声的一般非线性系统模型，
x
k
=
f
(
x
k
−
1
，
w
k
−
1
)
z
k
=
h
(
x
k
，
v
k
)
(2-1)
x_k=f(x_{k-1}， w_{k-1}) \\ z_k=h(x_k， v_k) \tag{2-1}
xk=f(xk−1，wk−1)zk=h(xk，vk)(2-1)
其中
x
k
x_k
xk为
k
k
k时刻的目标状态向量。
z
k
z_k
zk为
k
k
k时刻量测向量（传感器数据）。这里不考虑控制器
u
k
u_k
uk。
w
k
{w_k}
wk和
v
k
{v_k}
vk分别是过程噪声序列和量测噪声序列，并假设
w
k
w_k
wk和
v
k
v_k
vk为零均值高斯白噪声，其方差分别为
Q
k
Q_k
Qk和
R
k
R_k
Rk的高斯白噪声，即
w
k
∼
(
0
,
Q
k
)
w_k\sim(0,Q_k)
wk∼(0,Qk),
v
k
∼
(
0
,
R
k
)
v_k\sim(0,R_k)
vk∼(0,Rk)，且满足如下关系(线性高斯假设)为：
E
[
w
i
v
j
′
]
=
0
E
[
w
i
w
j
′
]
=
0
i
≠
j
E
[
v
i
v
j
′
]
=
0
i
≠
j
\begin{aligned} E[w_iv_j'] &=0\\ E[w_iw_j'] &=0\quad i\neq j \\ E[v_iv_j'] &=0\quad i\neq j \end{aligned}
E[wivj′]E[wiwj′]E[vivj′]=0=0i=j=0i=j
2.2 带非加性噪声的扩展卡尔曼滤波器（EKF）
1. 初始化
给定
k
−
1
k-1
k−1时刻的状态估计和协方差矩阵
x
^
k
−
1
∣
k
−
1
,
P
k
−
1
∣
k
−
1
,
Q
k
−
1
,
R
k
−
1
\hat{x}_{k-1|k-1},P_{k-1|k-1},Q_{k-1},R_{k-1}
x^k−1∣k−1,Pk−1∣k−1,Qk−1,Rk−1
当为
0
0
0时刻时，滤波器最优初始化为
x
0
∼
(
x
ˉ
0
,
P
0
)
,
Q
0
,
R
0
x
^
0
∣
0
=
x
ˉ
0
P
0
∣
0
=
P
0
x_0\sim(\bar{x}_0, P_0), Q_0,R_0\\\hat{x}_{0|0}=\bar{x}_0\\P_{0|0}=P_0
x0∼(xˉ0,P0),Q0,R0x^0∣0=xˉ0P0∣0=P0
2. 状态预测
2.1 计算非线性系统方程的雅可比矩阵
F
k
−
1
x
=
∂
f
(
x
k
−
1
,
w
k
−
1
)
∂
x
k
−
1
∣
x
k
−
1
=
x
^
k
−
1
∣
k
−
1
w
k
−
1
=
0
F
k
−
1
w
=
∂
f
(
x
k
−
1
,
w
k
−
1
)
∂
w
k
−
1
∣
x
k
−
1
=
x
^
k
−
1
∣
k
−
1
w
k
−
1
=
0
F_{k-1}^x=\frac{\partial f\left(x_{k-1},w_{k-1}\right)}{\partial x_{k-1}}\Big|_{{x_{k-1}=\hat{x}_{k-1|k-1}w_{k-1}=0}}\\ F_{k-1}^w=\frac{\partial f\left(x_{k-1},w_{k-1}\right)}{\partial w_{k-1}}\Big|_{{x_{k-1}=\hat{x}_{k-1|k-1}\\w_{k-1}=0}}
Fk−1x=∂xk−1∂f(xk−1,wk−1)∣
∣xk−1=x^k−1∣k−1wk−1=0Fk−1w=∂wk−1∂f(xk−1,wk−1)∣
∣xk−1=x^k−1∣k−1wk−1=0
2.2 状态一步预测及预测误差协方差阵为
x
^
k
∣
k
−
1
=
f
k
−
1
(
x
^
k
−
1
∣
k
−
1
,
0
)
P
k
∣
k
−
1
=
F
k
−
1
x
P
k
−
1
∣
k
−
1
(
F
k
−
1
x
)
′
+
F
k
−
1
w
Q
k
−
1
(
F
k
−
1
w
)
′
(2.2)
\textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k-1}&=f_{k-1}\left(\hat{x}_{k-1|k-1} ,0\right)\\ P_{k\mid k-1}&= F_{k-1}^xP_{k-1|k-1}(F_{k-1}^x)'+ F_{k-1}^wQ_{k-1}(F_{k-1}^w)' \end{aligned}} \tag{2.2}
x^k∣k−1Pk∣k−1=fk−1(x^k−1∣k−1,0)=Fk−1xPk−1∣k−1(Fk−1x)′+Fk−1wQk−1(Fk−1w)′(2.2)
其中系统状态方程在
x
^
k
−
1
∣
k
−
1
\hat{x}_{k-1|k-1}
x^k−1∣k−1的泰勒级数展开为（取一阶）
x
k
=
f
(
x
k
−
1
,
w
k
−
1
)
≈
f
(
x
^
k
−
1
∣
k
−
1
,
0
)
+
F
k
−
1
x
x
~
k
−
1
∣
k
−
1
+
F
k
−
1
w
w
k
−
1
\begin{aligned} x_k&=f\left(x_{k-1},w_{k-1} \right) \\ &\approx f\left(\hat{x}_{k-1|k-1}, 0\right)+F_{k-1}^x\widetilde{x}_{k-1|k-1}+F_{k-1}^ww_{k-1} \end{aligned}
xk=f(xk−1,wk−1)≈f(x^k−1∣k−1,0)+Fk−1xx
k−1∣k−1+Fk−1wwk−1
3. 量测预测
3.1 计算量测系统方程的雅可比矩阵
H
k
x
=
∂
h
(
x
k
,
v
k
)
∂
x
k
∣
x
k
=
x
^
k
∣
k
−
1
v
k
=
0
H
k
v
=
∂
h
k
(
x
k
,
v
k
)
∂
v
k
∣
x
k
=
x
^
k
∣
k
−
1
v
k
=
0
H_{k}^x=\frac{\partial h\left(x_{k},v_{k}\right)}{\partial x_{k}}\Big|_{{x_{k}=\hat{x}_{k|k-1}\\v_{k}=0}} \\ H_{k}^v=\frac{\partial h_{k}\left(x_{k},v_{k}\right)}{\partial v_{k}}\Big|_{{x_{k}=\hat{x}_{k|k-1}\\v_{k}=0}}
Hkx=∂xk∂h(xk,vk)∣
∣xk=x^k∣k−1vk=0Hkv=∂vk∂hk(xk,vk)∣
∣xk=x^k∣k−1vk=0
3.2 量测一步预测、新息协方差、互协方差为
z
^
k
∣
k
−
1
=
E
[
z
k
∣
Z
k
−
1
]
=
h
(
x
^
k
∣
k
−
1
,
0
)
S
k
=
cov
(
z
~
k
∣
k
−
1
)
=
H
k
x
P
k
∣
k
−
1
(
H
k
x
)
′
+
H
k
v
(
R
k
H
k
v
)
′
C
k
=
cov
(
x
~
k
∣
k
−
1
,
z
~
k
∣
k
−
1
)
=
P
k
∣
k
−
1
(
H
k
x
)
′
(2.3)
\textcolor{#FF0000}{ \begin{aligned} \hat{z}_{k|k-1}&=E\left[z_k\mid Z^{k-1}\right]= h\left(\hat{x}_{k|k-1},0\right)\\ S_k&=\text{cov}\left(\widetilde{z}_{k\mid k-1}\right)= H_{k}^xP_{k|k-1}(H_{k}^x)'+ H_{k}^v(R_{k}H_{k}^v)'\\ C_k&=\text{cov}\left(\widetilde{x}_{k\mid k-1},\widetilde{z}_{k\mid k-1}\right)= P_{k|k-1}(H_{k}^x)' \end{aligned}} \tag{2.3}
z^k∣k−1SkCk=E[zk∣Zk−1]=h(x^k∣k−1,0)=cov(z
k∣k−1)=HkxPk∣k−1(Hkx)′+Hkv(RkHkv)′=cov(x
k∣k−1,z
k∣k−1)=Pk∣k−1(Hkx)′(2.3)
其中量测方程在
x
^
k
∣
k
−
1
\hat{x}_{k|k-1}
x^k∣k−1的泰勒级数展开为（取一阶）
z
k
=
h
(
x
k
,
v
k
)
≈
h
k
(
x
^
k
∣
k
−
1
,
0
)
+
H
k
x
x
~
k
∣
k
−
1
+
H
k
v
v
k
−
1
\begin{aligned} z_k&=h\left(x_{k}, v_{k} \right)\\ &\approx h_{k}\left(\hat{x}_{k|k-1},0\right)+H_{k}^x\widetilde{x}_{k|k-1}+H_{k}^vv_{k-1} \end{aligned}
zk=h(xk,vk)≈hk(x^k∣k−1,0)+Hkxx
k∣k−1+Hkvvk−1
4. 状态更新
4.1 卡尔曼增益为
K
k
=
C
k
S
k
−
1
(2.4)
\textcolor{#FF0000}{ \begin{aligned} K_k&=C_kS_k^{-1} \end{aligned}} \tag{2.4}
Kk=CkSk−1(2.4)
4.2 状态更新为
x
^
k
∣
k
=
E
[
x
k
∣
Z
k
]
=
x
^
k
∣
k
−
1
+
K
z
(
z
k
−
z
^
k
∣
k
−
1
)
P
k
∣
k
=
cov
(
x
~
k
∣
k
)
=
P
k
∣
k
−
1
−
K
k
S
k
K
k
′
(2.5)
\textcolor{#FF0000}{ \begin{aligned} \hat{x}_{k|k}&=E\left[ x_k\mid Z^{k}\right ]=\hat{x}_{k|k-1}+K_z\left(z_k-\hat{z}_{k|k-1}\right)\\ P_{k\mid k}&=\text{cov}\left(\widetilde{x}_{k\mid k}\right)=P_{k\mid k-1}-K_kS_kK_k' \end{aligned}} \tag{2.5}
x^k∣kPk∣k=E[xk∣Zk]=x^k∣k−1+Kz(zk−z^k∣k−1)=cov(x
k∣k)=Pk∣k−1−KkSkKk′(2.5)
其中估计误差为
x
~
k
∣
k
=
x
k
−
x
^
k
∣
k
\widetilde{x}_{k\mid k}=x_k-\hat{x}_{k|k}
x
k∣k=xk−x^k∣k
以上公式（2.2-2.5）及为带非加性噪声非线性系统的扩展卡尔曼滤波算法。
三、仿真实验
扩展卡尔曼滤波 机动目标跟踪 EKF
/download/weixin_44044161/85401461
扩展卡尔曼滤波EKF匀速圆周运动CT
/download/weixin_44044161/85401885
EKF仿真代码：二维目标跟踪
/download/weixin_44044161/85123812
EKF仿真代码：三维目标跟踪
/download/weixin_44044161/85123744
仿真部分见博客：
扩展卡尔曼滤波EKF在目标跟踪中的应用—仿真部分
/weixin_44044161/article/details/115329181
3.1 仿真场景（三维）
EKF仿真代码：三维目标跟踪
/download/weixin_44044161/16239480
3.2 仿真场景（二维）
EKF仿真代码：二维目标跟踪
/download/weixin_44044161/16239356
3.3 二维EKF跟踪仿真结果
四、二维EKF跟踪参考代码
说明:
1.二维仿真代码也可以在上面的连接中直接下载，
2.将EKF函数保存，文件名“fun_2EKF.m”
3.将量测函数保存，文件名“”
4. 运行下面的主函数
5. 注意将这三个文件保存在一个文件夹下
4.1 主函数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% created by:
% date: 2020/4
% 扩展卡尔曼滤波，目标跟踪
% 二维目标跟踪问题
% 极坐标两侧
% 线性CV目标模型
% 单雷达
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all; close all; clc;
%% initial parameter
n=4; %状态维数 ;
T=1; %采样时间
M=1; %雷达数目
N=200; %运行总时刻
MC=500; %蒙特卡洛次数
chan=1; %滤波器通道，这里只有一个滤波器
w_mu=[0,0]'; % mean of process noise
v_mu=[0,0]'; % mean of measurement noise
%% target model
%covariance of process noise