27、协作导航方法：潜水员外部导航系统的优化与模拟

协作导航方法：潜水员外部导航系统的优化与模拟

1. 协作导航基础与EKF设计

在协作导航中，我们需要计算估计目标位置与参考目标（ROs）位置之间的距离。定义向量 $\hat{\mathbf{r}} = [\hat{r}_1 \cdots \hat{r}_n]^T$ 来表示这些距离，其元素为：
$\hat{r}_i(s_i) = \sqrt{(\hat{x}_0(s) - x_i(s_i))^2 + (\hat{y}_0(s) - y_i(s_i))^2}$

在首次运行时（声学信号到达第一个RO后），对于 $\hat{x}_0(s)$ 和 $\hat{y}_0(s)$ 使用先验估计，之后则始终使用最新的后验估计。向量 $\hat{\mathbf{r}}$ 与 $\mathbf{r}’$ 元素之间的差异是卡尔曼滤波器后验计算的必要输入。

由于目标和测量模型都具有非线性结构，因此使用扩展卡尔曼滤波器（EKF）进行持续跟踪。为此，需要计算非线性系统矩阵的雅可比矩阵 $\mathbf{F}$、$\mathbf{G}$、$\mathbf{H}$ 和 $\mathbf{M}$：
$\mathbf{F}(k) = \frac{\partial \mathbf{f}(k)}{\partial \mathbf{x}}\big| {\mathbf{x}=\hat{\mathbf{x}}^+(k)}$，$\mathbf{G}(k) = \frac{\partial \mathbf{f}(k)}{\partial \mathbf{w}}\big| {\mathbf{x}=\hat{\mathbf{x}}^+(k)}$
$\mathbf{H}(s) = \