[发明专利]一种基于非线性映射自适应混合Kalman/H∞滤波器的组合导航方法

权利要求书

1.一种基于非线性映射自适应混合Kalman/H∞滤波器的组合导航方法，包括以下几个步骤：

步骤一：在组合导航混合滤波器中，建立描述组合导航系统的状态方程和观测方程；

具体为，建立组合导航系统模型如下：

xk+1=Fk+1xk+Gk+1wkyk+1=Hk+1xk+1+vk+1zk+1=Lk+1xk+1---(1)]]>

其中，状态方程为x_k+1＝F_k+1x_k+G_k+1w_k，观测方程为y_k+1＝H_k+1x_k+1+v_k+1，待估计向量为z_k+1＝L_k+1x_k+1，k表示第k步，k+1表示第k+1步，x_k为第k步的n维状态向量，y_k+1为第k+1步的m维观测向量，z_k+1为第k+1步的q维待估计的输出向量，F_k+1为第k+1步系统状态转移矩阵，Η_k+1为第k+1步系统观测矩阵，G_k+1为第k+1步系统状态噪声矩阵，L_k+1为第k+1步估计矩阵，w_k为第k步n维系统噪声，v_k+1为第k+1步m维观测噪声，z_k+1为第k+1步待估计向量；

步骤二：在组合导航混合滤波器中同时运行卡尔曼滤波器和H∞滤波器；

卡尔曼滤波方程为：

假设w_k和v_k+1是不相关的零均值高斯白噪声，它们的方差分别为Q和R；组合导航系统的卡尔曼滤波器初始状态估计为表示初始状态估计，x₀表示组合导航系统真实的初始状态，0表示第0步，E表示期望，初始状态估计误差协方差为P₂₀表示初始状态估计误差协方差，卡尔曼滤波按照以下迭代方式进行：

x^2(k+1/k)=Fk+1x^2(k)]]>

P2(k+1/k)=Fk+1P2(k)Fk+1T+Gk+1QGk+1T]]>

K2(k+1)=P2(k+1/k)Hk+1T(Hk+1P2(k+1/k)Hk+1T+R)-1]]>

(2)

x^2(k+1)=x^2(k+1/k)+K2(k+1)(yk+1-Hk+1x^2(k+1/k))]]>

rk+1=yk+1-Hk+1x^2(k+1/k)]]>

z^2(k+1)=Lk+1x^2(k+1)]]>

P_2(k+1)＝(I-K_2(k+1)H_k+1)P_2(k+1/k)

其中：为卡尔曼滤波器第k步状态估计值，F_k+1为系统第k+1步状态转移矩阵，为卡尔曼滤波器第k+1步状态预测值，G_k+1为系统第k+1步状态噪声矩阵，P_2(k)为卡尔曼滤波器第k步状态估计误差协方差矩阵，P_2(k+1/k)为卡尔曼滤波器第k+1步状态预测误差协方差矩阵，H_k+1为系统第k+1步观测矩阵，K_2(k+1)为卡尔曼滤波器第k+1步增益矩阵，为卡尔曼滤波器第k+1步状态估计值，r_k+1为卡尔曼滤波器第k+1步新息，为待估计向量的第k+1步卡尔曼滤波估计值，I为n阶单位矩阵，P_2(k+1)为卡尔曼滤波器第k+1步状态估计误差协方差矩阵；当假设成立且系统模型准确时，卡尔曼滤波具有最小方差性能，可以获得对向量z_k+1精度较高的估计结果

H∞滤波方程为：

组合导航系统的H∞滤波器的初始滤波参数为：初始状态估计为初始估计广义协方差为P_∞0，对于给定的鲁棒参数γ＞0，如果[F_k+1G_k+1]行满秩，则H∞滤波器存在当且仅当对于所有的k有：

P∞(k)-1+HkTHk-γ-2LkTLk>0---(3)]]>

其中：P_∞(k)为H∞滤波第k步迭代矩阵，其迭代方式如下：

P∞(k+1)=FkP∞(k)FkT+GkGkT]]>

-FkP∞(k)HkTLkTRe,k-1HkLkP∞(k)FkT---(4)]]>

上式中，Re,k=I00-γ2I+HkLkP∞(k)HkTLkT,]]>I表示单位矩阵；

通过对式（4）进行矩阵代数运算，等价变换为：

P∞(k+1)=FkP∞k[I-γ-2LkTLkP∞k+HkTHkP∞k]-1FkT+GkGkT---(5)]]>

H∞滤波方程如下：

K∞(k+1)=P∞(k+1)Hk+1T(Hk+1P∞(k+1)Hk+1T+I)-1]]>

x^∞(k+1)=Fk+1x^∞(k)+Kk+1(yk+1-Hk+1Fk+1x^∞(k))---(6)]]>

z^∞(k+1)=Lk+1x^∞(k+1)]]>

其中：K_∞(k+1)为H∞滤波器第k+1步增益矩阵，和分别为H∞滤波器第k步和k+1步状态估计，P_∞(k+1)为H∞滤波器第k+1步迭代矩阵，为待估计向量z_k+1的第k+1步H∞滤波估计值；

步骤三：利用卡尔曼滤波器输出新息序列自相关矩阵的时间平均值和滤波器参数，获取卡尔曼滤波器性能量化指标；

具体为：

根据卡尔曼滤波器方程(2)，获取第i+1步新息r_i+1的函数值J_i+1：

Ji+1=ri+1Tri+1tr(Hi+1Pi+1/iHi+1T+R)---(7)]]>

其中，tr(·)表示矩阵的迹，对于卡尔曼滤波器的第k+1步迭代，建立以下卡尔曼滤波器性能量化指标：

J‾k+1=1MΣi=k-M+1k+1Ji+1---(8)]]>

其中：J_i+1由(7)式给出，为时间窗口宽度M内J_i+1的平均值，M为时间窗口宽度；

步骤四：建立卡尔曼滤波器性能量化指标与混合滤波器加权参数间的非线性映射关系，自适应地调整加权参数；

卡尔曼滤波器性能量化指标与加权参数d_k+1间的非线性映射函数如下：

dk+1=1(J‾k+1≤J2)be-J‾k+1a(J2<J‾k+1≤J∞)0(J‾k+1>J∞)---(9)]]>

其中：由式(8)计算给出，J₂和J_∞分别为卡尔曼滤波器上界运行参数和下界运行参数，a,b为参数；

步骤五：通过加权参数，将卡尔曼滤波器和H∞滤波器输出的加权和作为整个混合滤波器的输出，完成组合导航信息处理；

具体混合滤波器输出如下：

x^k+1=dk+1x^2(k+1)+(1-dk+1)x^∞(k+1)]]>

(10)

z^k+1=Lk+1x^k+1]]>

其中：为导航误差的估计值，为待求的导航误差值，通过公式（10）得到待求的导航误差值，通过对导航定位位置进行修正。