课题进度01-双平台纯方位目标跟踪

使用双平台被动纯方位角跟踪，目标做匀速直线运动，采用扩展卡尔曼滤波。

1. 参数设置

采样周期：1s

总仿真时间：100s

平台1位置：（-400，0），单位：m；速度：（0，1），单位：m/s

平台2位置：（400，0），单位：m；速度：（0，1），单位m/s

两个平台向正北方向前进

目标初始状态（x, y, vx, vy）：(100,100,-2,5)，朝着西北方向前进。

首先介绍一下卡尔曼滤波(KF)，它是将一个系统的观测值和预测值进行贝叶斯融合，得出一个最优估计，但是针对的只是线性系统，所以对于非线性系统就需要用到扩展卡尔曼滤波(EKF)。

扩展卡尔曼滤波(EKF)

用一阶泰勒展开将非线性系统近似为线性系统，然后在这个线性化的系统中使用传统的卡尔曼滤波进行状态更新。

状态转移方程（系统模型）:

$x_k=f(x_{k-1},u_{k-1})+w_{k-1}$ $f(x_{k-1})\approx f(\hat{x}_{k-1})+F_{k-1}(x_{k-1}-\hat{x}_{k-1})$

观测方程（测量模型）：

$z_k=h(x_k)+v_k$

$x_k$：系统在第k步的状态向量

$u_{k-1}$：控制输入

$w_{k-1}$：过程噪声，假设服从零均值，协方差为Q的高斯分布

$z_k$：测量值

$v_k$：测量噪声，服从协方差为R的高斯分布

$f()$ 和 $h()$ 是非线性函数

核心思想：非线性函数局部线性化

$f(x_{k-1})\approx f(\hat{x}_{k-1})+F_{k-1}(x_{k-1}-\hat{x}_{k-1})$

其中$F_{k-1}$是函数f对x的雅可比矩阵

$F_{k-1}=\left.\frac{\partial f}{\partial\mathbf{x}}\right|_{\mathbf{x}=\hat{\mathbf{x}}_{k-1},\mathbf{u}=\mathbf{u}_{k-1}}$

观测更新的线性化

同理，对$h(x_k)$在预测点$\hat{x}$附近一阶展开
$h(\mathbf{x}_k)\approx h(\hat{\mathbf{x}}_k)+H_k(\mathbf{x}_k-\hat{\mathbf{x}}_k)$

其中$H_k$是h对x的雅可比矩阵：

$H_k=\left.\frac{\partial h}{\partial\mathbf{x}}\right|_{\mathbf{x}=\hat{\mathbf{x}}_{k}}$