[发明专利]使用不变卡尔曼滤波器和第二交通工具的导航状态的交通工具导航辅助方法及装置

说明书

本发明涉及一种用于辅助第一交通工具(1)导航的方法，第一交通工具相对于在参考坐标系中可移动的第二交通工具(2)静止，所述方法包括：通过至少一个本体感受传感器(6)获取(112)第一交通工具(1)的移动数据；以及通过不变卡尔曼滤波器使用第二交通工具(2)的导航状态(X₂)作为观测值，估计第一交通工具(1)的导航状态(X₁)，第一交通工具(1)的导航状态包括：表示将与第一交通工具(1)相关联的第一位置标记关联到参考坐标系的刚性变换(T₁)的变量；以及表示将与第二交通工具(2)相关联的位置标记关联到第一位置标记的刚性变换(T₂₁)的变量，不变卡尔曼滤波器使用包括这两个刚性变换的逐项组合的法则作为内部组合法则。

技术领域

本发明涉及一种使用不变卡尔曼滤波器辅助载体导航的方法。

背景技术

估计物理系统的状态的问题通常如下所示。系统在时刻n的状态由向量X_n表示，以及在时刻n可用的观测值由另一个向量Y_n表示。系统的演化写成这样：

X_n+1＝f(X_n)，

其中，f是已知函数(通常称为传播函数)，可取决于传感器的测量值。观测值Y_n通过已知的观测函数h与系统的状态相关：

Y_n＝h(X_n)。

从序列(Y_n)_n≥0建立状态X_n的良好估计通常是个难题，但是在某些情况下可得以简化。

“线性系统”是指具有以下形式的系统的特殊情况：

X_n+1＝FX_n+w_n，

Y_n＝HX_n+V_n，

其中，F是传播矩阵，H是观测矩阵，w_n和V_n是干扰预测和测量的噪声。

在这种线性情况下，已知的方法包括建立称为“卡尔曼滤波器”的估计器。该卡尔曼滤波器实现以下计算：

其中，指数n+1|n和n+1|n+1分别表示在不考虑观测值Y_n+1的情况下，在n+1时刻计算的估计值，以及在考虑观测值Y_n+1的情况下，在n+1时刻计算的估计值。矩阵K_n称为“增益矩阵”，其可以使用Riccati(黎卡提)方程来计算。于是估计误差定义为：

(在考虑观测值Y_n之后)，

(在考虑观测值Y_n+1之前)。

可以容易地验证该误差遵循以下演化：

e_n+1|n＝Fe_n|n(在考虑观测值Y_n+1之前)，

e_n+1|n+1＝(I-K_n+1H)e_n+1|n(在考虑观测值Y_n+1之后)，其中，I表示单位矩阵。

上述方程不依赖于X_n，因此可以建立对系统的任何实际轨迹都有效的估计器，而对于任何非线性系统则不是这种情况。