CN112683270A - 一种基于平滑变结构滤波的gnss/sins/磁力计的组合方法 - Google Patents

一种基于平滑变结构滤波的gnss/sins/磁力计的组合方法 Download PDF

Info

Publication number
CN112683270A
CN112683270A CN202011432157.3A CN202011432157A CN112683270A CN 112683270 A CN112683270 A CN 112683270A CN 202011432157 A CN202011432157 A CN 202011432157A CN 112683270 A CN112683270 A CN 112683270A
Authority
CN
China
Prior art keywords
equation
sins
magnetometer
state
gnss
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202011432157.3A
Other languages
English (en)
Inventor
张秋昭
黄伟
孙澳
王方田
郑南山
李增科
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China University of Mining and Technology CUMT
Original Assignee
China University of Mining and Technology CUMT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China University of Mining and Technology CUMT filed Critical China University of Mining and Technology CUMT
Priority to CN202011432157.3A priority Critical patent/CN112683270A/zh
Publication of CN112683270A publication Critical patent/CN112683270A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

本发明提供一种基于平滑变结构滤波、利用磁力计和加速度计提供的姿态角差观测量来增强系统模型的可观测性，同时将平滑变结构滤波运用其中以提高状态变量的鲁棒性的GNSS/SINS/磁力计的组合方法，包括步骤：步骤一，在SINS系统中挑选合适的状态变量。步骤二，建立组合导航系统的状态方程。步骤三，建立组合导航系统的量测方程。步骤四，构建系统模型的可观测性矩阵，对系统模型进行可观测性分析；根据分析结果，评价基于GNSS/SINS/磁力计的组合系统是否具备完全可观测性；若是，则进行平滑变结构滤波的应用；若否，则重复步骤一到四，重新挑选状态变量和相应的观测量，设计新的状态方程和量测方程，直至系统为完全可观系统；步骤五，平滑变结构滤波的应用。

Description

一种基于平滑变结构滤波的GNSS/SINS/磁力计的组合方法
技术领域
本发明属于组合导航定位技术领域，具体涉及一种基于平滑变结构滤波的GNSS/SINS/磁力计的组合方法。
背景技术
GNSS/SINS组合导航是常用的导航手段之一：全球导航卫星系统(GNSS)具有误差不易发散，但卫星信号易受遮挡和干扰的特点；捷联惯导系统(SINS)不受外界干扰，自主性强，但误差容易积累。这两种导航系统结合取长补短，能够较好的完成导航任务。卡尔曼滤波是组合导航中常用的数据融合手段，但是该方法要求系统能够提供准确的状态方程和量测方程，且要求系统噪声和量测噪声为零均值协方差已知的高斯白噪声，否则难以满足要求。对于该问题。不少研究学者提出了扩展卡尔曼滤波(EKF)，无迹卡尔曼滤波(UKF)，粒子滤波(PF)等滤波方法。但或多或少存在一些不足，如将模型误差处理为系统噪声，滤波算法的鲁棒性不足等缺陷。
平滑变结构滤波(SVSF)最早在2007年提出。这种滤波基于滑模变结构控制，采用变结构增益将预测的状态变量纠正到系统真实状态附近，因此是一种纠正预测滤波。该滤波因其引进了变结构控制概念而具有较强的鲁棒性，对噪声和不确定性不敏感。但是该滤波也存在不足之处，它要求被估计的系统具有完全可观可控性。针对此不足，国内一些学者通过对组合导航系统模型的改进，使其完全可控可观。有学者设计了位置速度六维状态变量的组合导航模型；还有学者设计了一种降阶卡尔曼滤波组合对准方法。但是这些组合对准方法不包含完整的基本导航参数(位置、速度、姿态)，无法应用于组合导航定位中。
发明内容
本发明所要解决的技术问题在于克服现有技术缺陷，提供了一种基于平滑变结构滤波、利用磁力计和加速度计提供的姿态角差观测量来增强系统模型的可观测性，同时将平滑变结构滤波运用其中以提高状态变量的鲁棒性的GNSS/SINS/磁力计的组合方法。
为了解决上述技术问题，本发明提供的基于平滑变结构滤波的GNSS/SINS/磁力计的组合方法，包括步骤：
步骤一，在SINS系统中挑选合适的状态变量，包括惯导东北天向速度误差[δVE δVNδVU]、东北天向姿态角误差[φE φN φU]、惯导位置误差[δL δλ δh]。
步骤二，建立组合导航系统的状态方程
Figure BDA0002819685530000011
步骤三，建立组合导航系统的量测方程Z＝HX+V。
步骤四，构建系统模型的可观测性矩阵，对系统模型进行可观测性分析；根据分析结果，评价基于GNSS/SINS/磁力计的组合系统是否具备完全可观测性；若是，则进行平滑变结构滤波的应用；若否，则重复步骤一到四，重新挑选状态变量和相应的观测量，设计新的状态方程和量测方程，直至系统为完全可观系统；
步骤五，平滑变结构滤波的应用。
作为改进，步骤二中的组合导航系统的状态方程具体公式为：
Figure BDA0002819685530000012
X＝[δVE δVN δVU φE φN φU δL δλ δh]
Figure BDA0002819685530000021
其中，
Figure BDA0002819685530000022
Figure BDA0002819685530000023
Figure BDA0002819685530000024
Figure BDA0002819685530000025
Figure BDA0002819685530000026
上述公式中，F为状态转移矩阵，G为系统噪声分配矩阵，W为系统噪声向量；VE、VN、VU为惯导东北天向速度分量；ωN、ωU为北向和天向陀螺移角速率输出，fE、fN、fU为东北天向加速度计比力输出；L为地理纬度；h为地理高度；RMh＝RM+h，其中RM为子午圈主曲率半径；RNh＝RN+h，其中RN为卯酉圈主曲率半径；
Figure BDA0002819685530000027
Figure BDA0002819685530000028
β为重力扁率，f为椭球形状扁率；
Figure BDA0002819685530000029
μ为地心引力常数，R为地球平均半径，β2取值为3.08×10-6s-2；β3取值为8.08×10- 9s-2；将g0常记为ge，ge为地球赤道重力加速度。
作为改进，将加速度计偏置
Figure BDA00028196855300000212
和陀螺漂移ε从基本状态变量中删去，用等效白噪声取代，作为状态方程中的新增系统噪声，如下式：
Figure BDA00028196855300000210
Figure BDA00028196855300000211
状态方程中由系统噪声表达式推得G和W，如下式：
Figure BDA0002819685530000031
Figure BDA0002819685530000032
系统噪声中含有：陀螺零偏等效白噪声wg、加速度计偏置等效白噪声wa、陀螺仪测量白噪声
Figure BDA0002819685530000033
加速度计测量白噪声
Figure BDA0002819685530000034
作为改进，步骤三的量测方程具体构建步骤为：
加速度计和磁力计分别沿着载体坐标系三轴方向安装，在构造观测量之前，首先应该知道由加速度计计算出的俯仰角θ、横滚角γ，由磁力计输出的航向角ψ。在载体非加速运动状态下并且载体坐标系和参考坐标系重合时，可以得到加速度计在北东地参考坐标系下的比力输出为
fn＝[0 0 g]
地磁场强度在北东地参考坐标系下的输出为
Figure BDA0002819685530000035
当载体做非加速度运动时，加速度计在载体坐标系下的输出为
Figure BDA0002819685530000036
地磁场强度在载体坐标系下的输出为
Figure BDA0002819685530000037
其中fn为北东地坐标系下的加速度计比力输出，g为当地重力加速度，Hn为北东地坐标系下的磁力计三轴磁分量，上面四式存在以下关系：
Figure BDA0002819685530000038
Figure BDA0002819685530000039
上式中，q0、q1、q2、q3为四元数，
由上面两个方程可以得出以下方程组
Figure BDA00028196855300000310
对于上式，可得到q0、q1、q2、q3。
根据四元数和姿态角的转换关系，得到载体的俯仰角θ、横滚角γ和航向角ψ。
Figure BDA00028196855300000311
陀螺仪输出的载体系相对于惯性系的角速率为
Figure BDA00028196855300000312
与导航系相对于惯性系的角速率
Figure BDA00028196855300000313
相减后得到载体系相对于导航系的角速率
Figure BDA00028196855300000314
一次积分后的得到载体在导航系下的载体姿态角
Figure BDA0002819685530000041
其中i表示惯性坐标系，b表示载体坐标系，n表示导航坐标系，下标g表示该姿态角为陀螺输出的。
对于已经求出的两组姿态角观测量对其进行求差组成姿态角差观测量：
Figure BDA0002819685530000042
其次SINS编排输出的位置和速度观测量与GPS输出的位置和速度观测量的差值作为另一组观测量：
Figure BDA0002819685530000043
Figure BDA0002819685530000044
上式中，
Figure BDA0002819685530000045
为惯导速度分量；
Figure BDA0002819685530000046
为GPS的速度分量；Lsins、λsins、hsins、hgps、λgps、hgps分别为惯导和GPS纬度、经度和高度；vgps为GPS速度测量白噪声，Pgps为GPS位置测量白噪声，θgm为加速度计和磁力计测量白噪声。将上面两个观测量组合得到量测方程如下：
Figure BDA0002819685530000047
其中H矩阵为I9×9。
作为改进，步骤四的对所构建的系统模型进行可观测性判断方法为：
根据PWCS可观测性分析理论，在短时间内，可以认为组合导航系统是时不变的，因此取第j个时间间隔的可观测行矩阵Mj：
Figure BDA0002819685530000048
其中Fj为第j个时间间隔的状态转移矩阵，Hj为第j个时间间隔的量测转移矩阵。计算Mj的秩rank(Mj)，若rank(Mj)＝n，说明系统在第j个时间间隔完全可观测。如果rank(Mj)＜n，则说明系统不是完全可观测的，因此要重新设计组合系统。
作为改进，步骤五的所要运用的平滑变结构滤波基本公式为重写组合系统的状态方程和量测方程：
Figure BDA0002819685530000049
Z9×1＝H9×9X9×1+V9×1
下标表示矩阵的维度。
对上面两式运用平滑变结构滤波公式，
状态预测方程为：
Figure BDA00028196855300000410
记
Figure BDA0002819685530000051
为k-1时刻的状态最优估计，
Figure BDA0002819685530000052
为k时刻的状态先验估计。
状态误差协方差预测方程为：
Figure BDA0002819685530000053
记
Figure BDA0002819685530000054
为k-1时刻的状态误差协方差的最优估计，
Figure BDA0002819685530000055
为k时刻的状态误差协方差先验估计。Qk-1为k-1时刻的系统噪声方差。
新息方程为
Figure BDA0002819685530000056
k-1时刻的后验量测估计误差为
Figure BDA0002819685530000057
SVSF增益为
Figure BDA0002819685530000058
k时刻后验状态更新值为
Figure BDA0002819685530000059
k时刻后验状态误差协方差为
Figure BDA00028196855300000510
Rk为k时刻的量测噪声方差。
在SVSF增益公式中，
Figure BDA00028196855300000511
为Hk的广义逆；γ为收敛速度因子，0＜γ＜1；||为绝对值符号；“*”为矩阵点乘；
Figure BDA00028196855300000512
为Ψ-1的对角化，具体为
Figure BDA00028196855300000513
sat为饱和函数，具体形式为
Figure BDA00028196855300000514
Figure BDA00028196855300000515
的展开形式为
Figure BDA00028196855300000516
Figure BDA00028196855300000517
上式中Ψ为平滑边界层，令
Figure BDA00028196855300000518
则Ψ的取值为
Figure BDA0002819685530000061
Figure BDA0002819685530000062
。
本发明主要原理为：在考虑SVSF要求系统完全可控可观以及对基本导航参数的完全估计的情况下，首先挑选载体的位置误差、速度误差和姿态角误差参数作为状态变量，并将加速度计偏置和陀螺仪零偏作为系统噪声。然后根据此状态变量和系统噪声设计状态方程。接着在观测量中引入了姿态角差观测量，使得量测转移矩阵为九维满秩方阵。最终进行系统模型的可观测性分析和SVSF的应用。
有益效果：本发明引入磁力计/加速度计姿态角差观测量，使得系统模型完全可控可观，进而能够将SVSF应用在该组合方法中。同时本发明保证了导航基本参数的估计，新的组合方法和SVSF相结合，给基本导航参数提供了更好的鲁棒估计。
附图说明：
图1为本发明的流程图。
具体实施方式
下面根据附图对本发明作更进一步的说明
如图1所示，本发明的具体实施步骤如下：
步骤一：根据实际需求，挑选合适的导航参数，本发明挑选载体的惯导速度误差[δVE δVN δVU]，姿态角误差[φE φN φU]和惯导位置误差[δL δλ δh]作为状态变量：
X＝[δVE δVN δVU φE φN φU δL δλ δh]
步骤二：设计状态方程。为了满足SVSF的限制条件，将传统的15维滤波器削减为9维，其余6维度均作为等效系统白噪声处理。系统方程具体公式为
Figure BDA0002819685530000063
其中，
Figure BDA0002819685530000064
Figure BDA0002819685530000065
Figure BDA0002819685530000066
Figure BDA0002819685530000071
Figure BDA0002819685530000072
上述公式中，F为状态转移矩阵，G为系统噪声分配矩阵，W为系统噪声向量；VE、VN、VU为惯导东北天向速度分量；ωN、ωU为北向和天向陀螺移角速度输出，fE、fN、fU为东北天向加速度计比力输出；L为地理纬度；h为地理高度；RMh＝RM+h，其中RM为子午圈主曲率半径；RNh＝RN+h，其中RN为卯酉圈主曲率半径；
Figure BDA0002819685530000073
Figure BDA0002819685530000074
β为重力扁率，f为椭球形状扁率；
Figure BDA0002819685530000075
μ为地心引力常数，R为地球平均半径，β2取值为3.08×10-6s-2；β3取值为8.08×10- 9s-2；将g0常记为ge，ge为地球赤道重力加速度。
根据研究表明，消除加速度计偏置
Figure BDA00028196855300000712
和陀螺漂移ε这些有色噪声，在一定条件下能够保证系统的精度。因此为达到削减状态变量的目的，本发明将加速度计偏置
Figure BDA00028196855300000713
和陀螺漂移ε从基本状态变量中删除，并将其用等效白噪声取代，作为状态方程中的新增系统噪声，等效白噪声如下式：
Figure BDA0002819685530000076
Figure BDA0002819685530000077
显然，系统的精度会受到以上两个新增的等效系统噪声的影响。根据一些文献中的结论，当等效白噪声的方差设置为IMU有色噪声的3-5倍时，状态变量的估计精度和18维状态变量时相仿。此时，状态方程中可由系统噪声表达式推得G和W，如下式：
Figure BDA0002819685530000078
Figure BDA0002819685530000079
系统噪声中含有：陀螺零偏等效白噪声wg、加速度计偏置等效白噪声wa、陀螺仪测量白噪声
Figure BDA00028196855300000710
加速度计测量白噪声
Figure BDA00028196855300000711
步骤三：对观测量进行增量处理，使得量测转移矩阵H为9维满秩方阵，这是进行SVSF应用的关键一步。本发明选用磁力计和加速度计共同输出的姿态角作为间接观测量，然后与陀螺仪经过一次积分得到的姿态角进行求差得到姿态角差分观测量，最终和GNSS/SINS位置速度差分观测量进行融合，组成九维量测方程。具体实施步骤如下：
加速度计和磁力计三轴沿着载体系三轴方向安装。获取当地重力加速度和地磁场强度，在本发明中，当地重力加速度认为是1g，当地磁场强度可由WMM磁场模型查得。
在载体非加速度运动得情况下，可以得到：
加速度计在东北天参考坐标系下重力加速度分量
fn＝[0 0 g]
当磁地理坐标系m系和载体系b重合时，磁力计在北东地参考坐标系下得磁场强度分量
Figure BDA0002819685530000081
当载体运动时，加速度计在载体坐标系下的输出为
Figure BDA0002819685530000082
地磁场强度在载体坐标系下的三轴输出
Figure BDA0002819685530000083
其中fn为北东地坐标系下的加速度计比力输出，g为当地重力加速度，Hn为北东地坐标系下的磁力计三轴磁分量，上面四式存在以下关系：
Figure BDA0002819685530000084
Figure BDA0002819685530000085
由上面两个方程可以得出以下方程组
Figure BDA0002819685530000086
对于上式，可得到四元数q0、q1、q2、q3。
接着根据四元数和姿态角的转换关系，得到载体姿态角。
Figure BDA0002819685530000087
陀螺仪输出的载体系相对于惯性系的角速率为
Figure BDA0002819685530000088
与导航系相对于惯性系的角速率
Figure BDA0002819685530000089
相减后得到载体系相对于导航系的角速率
Figure BDA00028196855300000810
一次积分后的得到载体在导航系下的载体姿态角
Figure BDA00028196855300000811
对于已经求出的两组姿态角观测量对其进行求差组成姿态角差观测量：
Figure BDA00028196855300000812
其次SINS编排输出的位置和速度观测量与GPS输出的位置和速度观测量的差值作为另一组观测量：
Figure BDA0002819685530000091
V＝[vgps Pgps θgm]T
上式中，vgps为GPS速度测量白噪声，Pgps为GPS位置测量白噪声，θgm为加速度计和磁力计测量白噪声。将上面两个观测量组合得到量测方程如下：
Figure BDA0002819685530000092
其中H矩阵为I9×9。
步骤四：根据PWCS可观测性分析理论，在短时间内，可以认为组合导航系统是时不变的，因此取第j个时间间隔的可观测行矩阵Mj：
Figure BDA0002819685530000093
其中Fj为第j个时间间隔的状态转移矩阵，Hj为第j个时间间隔的量测转移矩阵。计算Mj的秩rank(Mj)，若rank(Mj)＝n，说明系统在第j个时间间隔完全可观测。如果rank(Mj)＜n，则说明系统不是完全可观测的，因此要重新设计组合系统。
步骤五：重写组合系统的状态方程和量测方程。
Figure BDA0002819685530000094
Z9×1＝H9×9X9×1+V9×1
对上面两式运用平滑变结构滤波公式，
状态预测方程为：
Figure BDA0002819685530000095
记
Figure BDA0002819685530000096
为k-1时刻的状态最优估计，
Figure BDA0002819685530000097
为k时刻状态变量的先验估计。
状态误差协方差预测方程为：
Figure BDA0002819685530000098
记
Figure BDA0002819685530000099
为k-1时刻的状态误差协方差的最优估计，
Figure BDA00028196855300000910
为k时刻的状态误差协方差先验估计。Qk-1为k-1时刻的系统噪声方差。
新息方程为
Figure BDA00028196855300000911
k-1时刻的后验量测估计误差为
Figure BDA00028196855300000912
SVSF增益为
Figure BDA00028196855300000913
后验状态更新值为
Figure BDA0002819685530000101
后验状态误差协方差为
Figure BDA0002819685530000102
在SVSF增益公式中，
Figure BDA0002819685530000103
为Hk的广义逆；γ为收敛速度因子，0＜γ＜1；||为绝对值符号；“*”为矩阵点乘；
Figure BDA0002819685530000104
为Ψ-1的对角化，具体为
Figure BDA0002819685530000105
sat为饱和函数，具体形式为
Figure BDA0002819685530000106
Figure BDA0002819685530000107
的展开形式为
Figure BDA0002819685530000108
Figure BDA0002819685530000109
上式中Ψ为平滑边界层，令
Figure BDA00028196855300001010
则Ψ的取值为
Figure BDA00028196855300001011
Figure BDA00028196855300001012
。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下还可以做出若干改进，这些改进也应视为本发明的保护范围。

Claims (6)

1.一种基于平滑变结构滤波的GNSS/SINS/磁力计组合方法，其特征在于，包括以下步骤：
步骤一，在SINS系统中挑选状态变量，包括惯导速度误差[δVE δVN δVU]、姿态角误差[φE φN φU]、惯导位置误差[δL δλ δh]；
步骤二，建立组合导航系统的状态方程
Figure FDA0002819685520000017
步骤三，建立组合导航系统的量测方程Z＝HX+V；
步骤四，构建系统模型的可观测性矩阵，对系统模型进行可观测性分析；根据分析结果，评价基于GNSS/SINS/磁力计的组合系统是否具备完全可观测性；若是，则进行平滑变结构滤波的应用；若否，则重复步骤一到四，重新挑选状态变量和相应的观测量，设计新的状态方程和量测方程，直至系统为完全可观系统；
步骤五，平滑变结构滤波的应用。
2.根据权利要求1所述的基于平滑变结构滤波的GNSS/SINS/磁力计组合方法，其特征在于：所述步骤二的组合导航系统的状态方程中：
Figure FDA0002819685520000011
其中，
Figure FDA0002819685520000012
Figure FDA0002819685520000013
Figure FDA0002819685520000014
Figure FDA0002819685520000015
Figure FDA0002819685520000016
3.根据权利要求2所述的基于平滑变结构滤波的GNSS/SINS/磁力计组合方法，其特征在于：将加速度计偏置
Figure FDA0002819685520000018
和陀螺漂移ε从基本状态变量中删去，用等效白噪声取代，作为状态方程中的新增系统噪声，如下式：
Figure FDA0002819685520000021
Figure FDA0002819685520000022
状态方程中可由系统噪声表达式推得G和W，如下式：
Figure FDA0002819685520000023
Figure FDA0002819685520000024
系统噪声中含有：陀螺零偏等效白噪声wg、加速度计偏置等效白噪声wa、陀螺仪测量白噪声
Figure FDA0002819685520000025
加速度计测量白噪声
Figure FDA0002819685520000026
4.根据权利要求1所述的基于平滑变结构滤波的GNSS/SINS/磁力计组合方法其特征在于，所述步骤三的量测方程的具体构建步骤为：
加速度计和磁力计分别沿着载体坐标系三轴方向安装，在构造观测量之前，确定俯仰角θ、横滚角γ及航向角ψ；在载体非加速运动状态下并且载体坐标系和参考坐标系重合时，得到加速度计在北东地参考坐标系下的比力输出为
fn＝[0 0 g]
地磁场强度在北东地参考坐标系下的输出为
Figure FDA0002819685520000027
当载体运动时，加速度计在载体坐标系下的输出为
Figure FDA0002819685520000028
地磁场强度在载体坐标系下的输出为
Figure FDA0002819685520000029
其中fn为北东地坐标系下的加速度计比力输出，g为当地重力加速度，Hn为北东地坐标系下的磁力计三轴磁分量，上面四式存在以下关系：
Figure FDA00028196855200000210
Figure FDA00028196855200000211
由上面两个方程可以得出以下方程组
Figure FDA00028196855200000212
对于上式，可得到q0、q1、q2、q3；
根据四元数和姿态角的转换关系，得到载体姿态角；
Figure FDA0002819685520000031
陀螺仪输出的载体系相对于惯性系的角速率为
Figure FDA0002819685520000032
与导航系相对于惯性系的角速率
Figure FDA0002819685520000033
相减后得到载体系相对于导航系的角速率
Figure FDA0002819685520000034
一次积分后的得到载体在导航系下的载体姿态角
Figure FDA0002819685520000035
对于已经求出的两组姿态角观测量对其进行求差组成姿态角差观测量：
Figure FDA0002819685520000036
其次SINS编排输出的位置和速度观测量与GPS输出的位置和速度观测量的差值作为另一组观测量：
Figure FDA0002819685520000037
V＝[vgps Pgps θgm]T
上式中，vgps为GPS速度测量白噪声，Pgps为GPS位置测量白噪声，θgm为加速度计和磁力计测量白噪声。将上面两个观测量组合得到量测方程如下：
Figure FDA0002819685520000038
其中H矩阵为I9×9。
5.根据权利要求3或4所述的基于平滑变结构滤波的GNSS/SINS/磁力计组合方法，其特征在于，所述步骤四中对所构建的系统模型进行可观测性判断方法为：取第j个时间间隔的可观测性矩阵Mj：
Figure FDA0002819685520000039
计算Mj的秩rank(Mj)，若rank(Mj)＝n，说明系统在第j个时间间隔完全可观测；如果rank(Mj)＜n，说明系统不是完全可观测的，重新设计组合系统。
6.根据权利要求书1所述的基于平滑变结构滤波的GNSS/SINS/磁力计组合方法，其特征在于：所述步骤五中所要运用的平滑变结构滤波基本公式为：重写组合系统的状态方程和量测方程：
Figure FDA00028196855200000310
Z9×1＝H9×9X9×1+V9×1
对上面两式运用平滑变结构滤波公式：
状态预测方程为
Figure FDA0002819685520000041
误差协方差预测方程为
Figure FDA0002819685520000042
新息方程为
Figure FDA0002819685520000043
k-1时刻的后验量测估计为
Figure FDA0002819685520000044
SVSF增益公式为
Figure FDA0002819685520000045
后验状态更新：
Figure FDA0002819685520000046
后验误差协方差为：
Figure FDA0002819685520000047
在SVSF增益公式中，
Figure FDA0002819685520000048
为Hk的广义逆；γ为收敛速度因子，0＜γ＜1；||为绝对值符号；“*”为矩阵点乘；
Figure FDA0002819685520000049
为Ψ-1的对角化，具体为
Figure FDA00028196855200000410
sat为饱和函数，具体形式为
Figure FDA00028196855200000411
Figure FDA00028196855200000412
的展开形式为
Figure FDA00028196855200000413
Figure FDA00028196855200000414
上式中Ψ为平滑边界层，令
Figure FDA00028196855200000415
则Ψ的取值为
Figure FDA00028196855200000416
Figure FDA0002819685520000051
CN202011432157.3A 2020-12-07 2020-12-07 一种基于平滑变结构滤波的gnss/sins/磁力计的组合方法 Pending CN112683270A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011432157.3A CN112683270A (zh) 2020-12-07 2020-12-07 一种基于平滑变结构滤波的gnss/sins/磁力计的组合方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011432157.3A CN112683270A (zh) 2020-12-07 2020-12-07 一种基于平滑变结构滤波的gnss/sins/磁力计的组合方法

Publications (1)

Publication Number Publication Date
CN112683270A true CN112683270A (zh) 2021-04-20

Family

ID=75446517

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011432157.3A Pending CN112683270A (zh) 2020-12-07 2020-12-07 一种基于平滑变结构滤波的gnss/sins/磁力计的组合方法

Country Status (1)

Country Link
CN (1) CN112683270A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113137977A (zh) * 2021-04-21 2021-07-20 扬州大学 一种sins/偏振光组合导航初始对准滤波方法
CN115451952A (zh) * 2022-08-29 2022-12-09 南京航空航天大学 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103017763A (zh) * 2011-09-20 2013-04-03 雅马哈株式会社 状态估计设备和偏移更新方法
US20190126474A1 (en) * 2016-04-27 2019-05-02 Polarworks As Method of iterative motion control
CN109945895A (zh) * 2019-04-09 2019-06-28 扬州大学 基于渐消平滑变结构滤波的惯性导航初始对准方法
CN110764523A (zh) * 2019-11-13 2020-02-07 中国人民解放军海军航空大学 基于抗饱和平滑变换的比例积分前置导引攻击目标的方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103017763A (zh) * 2011-09-20 2013-04-03 雅马哈株式会社 状态估计设备和偏移更新方法
US20190126474A1 (en) * 2016-04-27 2019-05-02 Polarworks As Method of iterative motion control
CN109945895A (zh) * 2019-04-09 2019-06-28 扬州大学 基于渐消平滑变结构滤波的惯性导航初始对准方法
CN110764523A (zh) * 2019-11-13 2020-02-07 中国人民解放军海军航空大学 基于抗饱和平滑变换的比例积分前置导引攻击目标的方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
FARIZ OUTAMAZIRT等: "A new SINS/GPS sensor fusion scheme for UAV localization problem using nonlinear SVSF with covariance derivation and an adaptive boundary layer", 《CHINESE JOURNAL OF AERONAUTICS》, 23 February 2016 (2016-02-23), pages 425 - 440 *
FARIZ OUTAMAZIRT等: "Comparing between the Performance of SVSF with EKF and NH∞ for the Autonomous Airborne Navigation Problem", 《2016 IEEE AEROSPACE CONFERENCE》, 30 June 2016 (2016-06-30), pages 1 - 8 *
成光浩: "基于抗干扰滤波的捷联惯导系统初始对准研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》, 15 February 2020 (2020-02-15), pages 135 - 510 *
施常勇;张丽敏;: "平滑变结构滤波在航天器组合导航中的应用", 中国惯性技术学报, no. 04, 15 August 2017 (2017-08-15) *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113137977A (zh) * 2021-04-21 2021-07-20 扬州大学 一种sins/偏振光组合导航初始对准滤波方法
CN115451952A (zh) * 2022-08-29 2022-12-09 南京航空航天大学 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置
CN115451952B (zh) * 2022-08-29 2023-11-07 南京航空航天大学 一种故障检测及抗差自适应滤波的多系统组合导航方法和装置

Similar Documents

Publication Publication Date Title
CN109556632B (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN107655476B (zh) 基于多信息融合补偿的行人高精度足部导航方法
CN109459044B (zh) 一种gnss双天线辅助的车载mems惯导组合导航方法
Wu et al. Velocity/position integration formula part I: Application to in-flight coarse alignment
US6860023B2 (en) Methods and apparatus for automatic magnetic compensation
CN102169184B (zh) 组合导航系统中测量双天线gps安装失准角的方法和装置
CN109556631B (zh) 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法
CN108827310A (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN110514203B (zh) 一种基于isr-ukf的水下组合导航方法
CN110243377B (zh) 一种基于分层式结构的集群飞行器协同导航方法
CN109945895B (zh) 基于渐消平滑变结构滤波的惯性导航初始对准方法
CN104698485A (zh) 基于bd、gps及mems的组合导航系统及导航方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
CN112683270A (zh) 一种基于平滑变结构滤波的gnss/sins/磁力计的组合方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN109000639B (zh) 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置
Fu et al. In-motion alignment for a velocity-aided SINS with latitude uncertainty
CN111121766A (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN104166169A (zh) 一种捷联于水下潜器的三轴磁场传感器误差补偿方法
Xue et al. In-motion alignment algorithm for vehicle carried SINS based on odometer aiding
CN111290007A (zh) 一种基于神经网络辅助的bds/sins组合导航方法和系统
CN112325886A (zh) 一种基于重力梯度仪和陀螺仪组合的航天器自主定姿系统
CN108151765B (zh) 一种在线实时估计补偿磁强计误差的定位测姿方法
CN111024074A (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination