[发明专利]一种适用于微惯性与全球定位组合导航系统的分布式滤波装置

权利要求书

1.一种适用于微惯性与全球定位组合导航系统的分布式滤波装置，MIMU进行惯性

导航解算后得到速度、姿态和位置信息，GPS提供航向和速度信息，其特征在于：

该分布式滤波装置中包含5个差值比较单元和5个滤波器；

所述的5个差值比较单元是指A差值比较单元（11）、B差值比较单元（21）、 C差值比较单元（31）、D差值比较单元（41）和E差值比较单元（51）；

所述的5个滤波器是指一阶东向速度滤波器（10）、一阶北向速度滤波器（20）、 七阶姿态卡尔曼滤波器（30）、一阶东向位置滤波器（40）和一阶北向位置滤波器 （50）；

该分布式滤波装置利用MIMU和GPS的航向、速度信息作为量测输入进行滤波， 滤波所得到的信息反馈到MIMU中进行校正，最后经过校正后的速度、姿态和位置 信息作为MIMU/GPS组合导航系统的输出；

A差值比较单元（11）用于接收微惯性测量系统输出的东向速度和全球定位 系统输出的东向速度并将东向速度差值输出给一阶东向速度滤 波器（10）；

一阶东向速度滤波器（10）一方面对接收的δV_E依据预测/滤波关系 V10=δVE>2PDOP>7orSV<7]]>进行预测或者滤波判断，若满足预测/滤波关系则进行预测处 理；若不满足预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F₁₀处 理后输出东向速度误差估计值PDOP表示位置精度强弱度，SV表示GPS可见 星的数目；所述的滤波方程为F10=X^k=X^k-1+Pk-1+QPk-1+Q+R(Zk-X^k-1)Pk=(Pk-1+Q)RRk-1+Q+R,]]>表示当前时刻k 的状态估计值；表示前一时刻k-1的状态估计值；P_k表示当前时刻k的误差协方 差矩阵；P_k-1表示前一时刻k-1的误差协方差矩阵；Q表示MIMU/GPS组合导航系 统下的噪声矩阵；R表示量测噪声矩阵；Z_k表示当前时刻k的量测变量；

在预测模式下，一阶东向速度滤波器（10）输出的东向速度误差预测值受 噪声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则东向速 度滤波器的滤波方程为P10=X^k=X^k-1Pk=Pk-1+Q;]]>

B差值比较单元（21）用于接收微惯性测量系统输出的北向速度和全球定位 系统输出的北向速度并将北向速度差值输出给一阶北向速度滤 波器（20）；

一阶北向速度滤波器（20）一方面对接收的δV_N依据预测/滤波关系 V20=δVE>2PDOP>7orSV<7]]>进行预测或者滤波判断，若满足预测/滤波关系则进行预测处 理；若不满足预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤波方程F₂₀处 理后输出北向速度误差估计值所述的滤波方程为 F20=X^k=X^k-1+Pk-1+QPk-1+Q+R(Zk-X^k-1)Pk=(Pk-1+Q)RRk-1+Q+R;]]>

在预测模式下，一阶北向速度滤波器（20）输出的北向速度误差预测值受噪 声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则北向速度 滤波器的滤波方程为P20=X^k=X^k-1Pk=Pk-1+Q;]]>

C差值比较单元（31）用于接收微惯性测量系统输出的东向速度和北向速度 以及全球定位系统输出的东向速度和北向速度并将东向速度差值 和北向速度差值输出给七阶姿态卡尔曼滤波器（30）；

七阶姿态卡尔曼滤波器（30）中的状态变量X=δVEδVNΦEΦNΦupϵxϵy,]]>量测变量Z=δVEδVN,]]>则状态方程X·=FX+ΓW,]]>量测方程Z·=HX+V;]]>其中：

000-gfN0000g0-fE000-1/Re000001/Re000000000000000000-1/τr0000000-1/τr.]]>

Γ=1000000010000000100000001000000010000000100000001.W=w1w2w3w4w5w6w7.H=10000000100000]]>和V=vEvN;]]>Φ_E表示东向误差角；Φ_N表示北向误差角；Φ_UP表示方位失准角；ε_x表示X轴陀螺 漂移量；ε_y表示Y轴陀螺漂移量；Re表示地球半径；g表示重力加速度；f_E表示东 向等效比力；f_N表示北向等效比力；τ_r表示陀螺漂移马尔科夫过程的时间常数；w₁表 示加表引起的输入噪声的东向分量；w₂表示加表引起的输入噪声的北向分量；w₃表 示X轴陀螺的输入噪声；w₄表示Y轴陀螺的输入噪声；w₅表示Z轴陀螺的输入噪声； w₆表示X轴陀螺的马尔科夫噪声；w₇表示Y轴陀螺的马尔科夫噪声；v^E表示东向速 度误差噪声；v^N表示北向速度误差噪声；

七阶姿态卡尔曼滤波器（30）对接收的δV_E和δV_N进行卡尔曼滤波方法处理后输 出姿态误差估计值Φ^=Φ^EΦ^NΦ^UP]]>和陀螺漂移估计值ϵ^=ϵ^xϵ^y;]]>表示东向误差角估计 值；表示北向误差角估计值；表示方位失准角估计值；表示X轴陀螺漂移 估计值；表示Y轴陀螺漂移估计值；

D差值比较单元（41）用于接收微惯性测量系统输出的东向位置E^INS和全球定 位系统输出的东向位置E^GPS，并将东向位置差值δE＝E^INS-E^GPS输出给一阶东向位置 滤波器（40）；

一阶东向位置滤波器（40）一方面对接收的δE依据位置预测/滤波关系 V₄₀＝{PDOP＞7 or SV＜7}进行预测或者滤波判断，若满足位置预测/滤波关系则进行 预测处理；若不满足位置预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤 波方程F₄₀处理后输出东向位置误差估计值所述的滤波方程 F40=X^k=X^k-1+Pk-1+QPk-1+Q+R(Zk-X^k-1)Pk=(Pk-1+Q)RRk-1+Q+R;]]>

在预测模式下，一阶东向位置滤波器（40）输出的东向位置误差预测值受噪 声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则东向位置 滤波器的滤波方程为P40=X^k=X^k-1Pk=Pk-1+Q;]]>

E差值比较单元（51）用于接收微惯性测量系统输出的北向位置N^INS和全球定 位系统输出的北向位置N^GPS，并将北向位置差值δN＝N^INS-N^GPS输出给一阶北向位置 滤波器（50）；

一阶北向位置滤波器（50）一方面对接收的δN依据位置预测/滤波关系 V₅₀＝{PDOP＞7 or SV＜7}进行预测或者滤波判断，若满足位置预测/滤波关系则进行 预测处理；若不满足位置预测/滤波关系则进行滤波处理；另一方面，滤波时采用滤 波方程F₅₀处理后输出北向位置误差估计值所述的滤波方程 F50=X^k=X^k-1+Pk-1+QPk-1+Q+R(Zk-X^k-1)Pk=(Pk-1+Q)RRk-1+Q+R;]]>

在预测模式下，一阶北向位置滤波器（50）输出的北向位置误差预测值受噪 声方差和滤波增益的影响，即噪声方差趋于无穷大，滤波增益近似为零，则北向位置 滤波器的滤波方程为P50=X^k=X^k-1Pk=Pk-1+Q.]]>