使用卡尔曼滤波进行dvl标定

1. 状态向量定义

状态向量专门用于估计 DVL 的各类误差。我们有以下三类误差：

• 常值误差（零偏误差）：即 DVL 在测量速度时的恒定偏差。
• 刻度因数误差：即 DVL 测量速度值的比例误差。
• 安装误差：DVL 与载体之间的安装角度误差。

因此，状态向量可以定义为：

$$X_k=\left[\begin{array}{c}{b}_x\\ b_y\\ b_z\\ {\alpha }_x\\ {\alpha }_y\\ {\alpha }_z\\ {\theta }_x\\ {\theta }_y\\ {\theta }_z\end{array}\right]$$

---

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

系统模型描述的是 DVL 误差的演化。因为我们假设这些误差在短时间内是恒定的，状态向量不会随时间有显著变化，因此状态转移方程可以是简单的恒定模型：

$$X_{k+1}=X_k$$

这意味着我们认为 DVL 的安装误差、刻度因数误差和常值误差在整个过程中保持恒定，或变化很小。

在卡尔曼滤波器的状态更新中，忽略了过程噪声 ( w_k )，因此状态预测方程简化为：

$${\hat{X}}_{k+1}=A{\hat{X}}_k$$

其中 ( A ) 为单位矩阵 ( I )。

---

3. 测量模型

测量模型描述的是 DVL 的观测如何受到误差的影响。这里，DVL 的测量值 ( Z_k ) 会受到常值误差、刻度因数误差和安装误差的影响。设 DVL 测量的是速度 ( V_{DVL} )，而外部参考传感器（如 GPS）提供的“真值”速度是 ( V_{ref} )，则 DVL 测量模型可以表示为：

$$Z_k=H{X}_k+v_k$$

其中 ( H ) 为测量矩阵，表示 DVL 测量受各种误差的影响。具体的测量模型可以描述为：

$$\left[\begin{array}{c}{V}_{DVLx}\\ V_{DVLy}\\ V_{DVLz}\end{array}\right]=\left[\begin{array}{c}(1+{\alpha }_x)(V_{refx}+b_x)+{ϵ}_x({\theta }_x)\\ (1+{\alpha }_y)(V_{refy}+b_y)+{ϵ}_y({\theta }_y)\\ (1+{\alpha }_z)(V_{refz}+b_z)+{ϵ}_z({\theta }_z)\end{array}\right]+v_k$$

---

4. 卡尔曼滤波步骤

通过卡尔曼滤波器，可以不断调整 DVL 的误差估计。具体步骤如下：

• 预测步骤：

  • 预测状态（根据状态转移方程）：

    $${\hat{X}}_{k+1}=A{\hat{X}}_k={\hat{X}}_k$$

  • 预测误差协方差：

    $$P_{k+1}=A{P}_k{A}^T=P_k$$

• 更新步骤：

  • 计算卡尔曼增益：

    $$K_k=P_k{H}^T(H{P}_k{H}^T+R{)}^{-1}$$

    其中，( R ) 是测量噪声协方差矩阵。

  • 更新状态估计：

    $${\hat{X}}_k={\hat{X}}_k+K_k(Z_k-H{\hat{X}}_k)$$

    这里的 ( Z_k ) 是 DVL 的测量数据，( V_{ref} ) 是外部传感器提供的参考速度。

  • 更新误差协方差矩阵：

    $$P_k=(I-K_{k}H)P_k$$

---

5. 结果解释

通过卡尔曼滤波器的状态更新，逐渐估计出 DVL 的常值误差、刻度因数误差和安装误差。外部传感器提供的参考信息作为真值，用来更新卡尔曼滤波器的状态，从而使得 DVL 的测量结果趋于更准确。

---

6. 注意事项

• 外部参考的准确性：虽然假设外部传感器提供的参考速度是真值，但在实际应用中，外部传感器也可能有噪声或误差。如果外部传感器误差较大，可能会影响最终的 DVL 标定结果。
• 噪声矩阵 ( R )：根据实际测量数据，合理选择测量噪声协方差矩阵 ( R )，有助于滤波器的收敛和稳定性。
• 非线性误差处理：安装误差 ( \theta ) 可能引入非线性关系，如果误差较大，可以考虑使用扩展卡尔曼滤波器（EKF）或无迹卡尔曼滤波器（UKF）来处理非线性问题。