CN107643534A - 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 - Google Patents
一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 Download PDFInfo
- Publication number
- CN107643534A CN107643534A CN201710811405.7A CN201710811405A CN107643534A CN 107643534 A CN107643534 A CN 107643534A CN 201710811405 A CN201710811405 A CN 201710811405A CN 107643534 A CN107643534 A CN 107643534A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- msubsup
- mtd
- error
- 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.)
- Granted
Links
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，包括以下步骤：1根据载体的初始位置、速率和姿态信息构建状态方程，初始化卡尔曼滤波的参数；2进行M步步长的状态预测更新，得到先验状态量的预测值3对先验状态量进行修正，得到后验状态量的预测值4对状态量的误差和系统误差协方差矩阵进行自适应更新，并用后验状态量预测值对惯性导航结果进行补偿，得到载体位置、速度与姿态信息；5补偿完成后更新该方法可以在GNSS/INS深组合导航的数据融合算法过程中，降低因GNSS卫星数据更新频率低或卫星数据失锁导致的截断误差；同时解决因INS数据与GNSS数据不同步导致的导航定位误差。
Description
技术领域
本发明属于组合导航领域，具体涉及到一种适用于GNSS/INS深组合导航的双速率卡尔曼滤波方法。
背景技术
由于GNSS和INS优缺点的互补性，使得GNSS/INS组合导航成为了一种常见的连续、高带宽、长时和短时精度均较高的组合导航方式。GNSS抑制了INS的长时间发散问题，而INS则对GNSS结果进行了平滑并弥补了卫星信号中断时的不连续性。GNSS/INS组合导航系统的组合方式有许多种，区别主要在于对导航参数的校正、GNSS测量的类型以及算法对单独子系统的辅助方式上。深组合将GNSS/INS和GNSS信号跟踪融合在一起，由于辅助捕获为GNSS测距处理器提供了近似的速度和位置，因而减少了捕获过程所需要的搜索单元个数，可以允许很长的驻留时间。采用惯性导航参数辅助跟踪环路还可以使环路带宽更窄，进而提高对噪声的抑制。
现有的组合导航算法一般在每次闭环卡尔曼滤波补偿后都默认失准角、速度误差、位置误差等状态量为0，且系统在离散化过程中选用一阶截断会造成截断误差，因而会不可避免地造成误差。
发明内容
发明目的：针对现有技术中存在的问题，本发明提供了一种基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，该方法可以在GNSS/INS深组合导航的数据融合算法过程中，降低因GNSS卫星数据更新频率低或卫星数据失锁导致的截断误差；同时解决因INS数据与GNSS数据不同步导致的导航定位误差。
技术方案：本发明采用如下技术方案：
一种基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，包括以下步骤：
(1)根据载体的初始位置、速率和姿态信息构建状态方程，初始化卡尔曼滤波的协方差误差矩阵P0、系统噪声协方差矩阵Q0、观测误差矩阵R0；
所述状态方程为：
其中为载体系到ECEF系的失准角，为ECEF系下的速度误差，和为ECEF系下的位置误差，ba为加速度计偏置，bg为陀螺仪偏置，δtu为接收机的钟差，δtru为接收机的钟漂；T表示矩阵的转置；
(2)进行M步步长的状态预测更新，得到先验状态量的预测值M为惯性数据和卫星数据更新频率之比；根据外推得到载体当前位置和速度信息；
(3)对先验状态量进行修正，得到后验状态量的预测值根据外推得到载体当前位置和速度信息；
(4)对状态量的误差qk+M和系统误差协方差矩阵Qk+M进行自适应更新，并用后验状态量预测值对惯性导航结果进行补偿，得到载体位置、速度与姿态信息；
(5)补偿完成后更新更新方程为：
步骤(2)中状态更新方程为：
其中k＝0,1…,M-1，为第k+1步的先验状态量预测值；为第k步的后验状态量预测值；ΦS,k为第k步更新时的离散化状态转移矩阵，为当前惯导解算得到的经度Lon、纬度Lat和惯性数据两次更新之间的时间间隔τINS的函数。
步骤(3)中更新方程为：
其中ΦO,k为第k步的观测更新的状态转移矩阵，为当前惯导解算得到的经度Lon、纬度Lat和卫星数据的更新时间τGNSS的函数；为第k步的卡尔曼滤波的后验协方差误差矩阵预测值；为第k+M步的卡尔曼滤波的先验协方差误差矩阵预测值；为第k+M步的卡尔曼滤波的后验协方差误差矩阵预测值；Qk为第k步的卡尔曼滤波的系统噪声协方差矩阵；Hk+M为第k+M步的状态向量的H矩阵；R为卡尔曼滤波的观测误差矩阵；(·)-1为矩阵求逆运算符；I为单位矩阵；Zk+M为第k+M步的卡尔曼滤波的观测值。
步骤(4)中状态量的误差qk+M和系统误差协方差矩阵Qk+M进行自适应更新的方程为：
其中b为遗忘因子，取值范围为(0.95,0.99)。
用后验状态量预测值对惯性导航结果进行补偿，包括如下步骤：
(4.1)根据和状态方程得到载体系到ECEF系的失准角ECEF系下的速度误差ECEF系下的位置误差加速度计偏置ba，陀螺仪偏置bg，接收机的钟差δtu和接收机的钟漂δtru；
(4.2)从载体系到ECEF坐标系的变换矩阵为可以得到补偿后的变换矩阵将ECEF坐标系下的速度、位置值减去和得到载体的速度位置值；将上一时刻加速度计偏置和陀螺仪偏置分别于ba和bg相加得到当前加速度计偏置和陀螺仪偏置。
为防止卫星数据与惯性数据的不同步引起的误差，步骤(2)还包括对先验状态量的预测值的进一步修正，修正方程为：
其中为修正前的先验状态量的预测值，为修正后的先验状态量的预测值，Φ′S,k为第k步的观测更新的状态修正转移矩阵，为当前惯导解算得到的经度Lon、纬度Lat、卫星数据所在时刻与该卫星数据之前最近一次惯导数据所在时刻的时间差△τm的函数。
有益效果：与现有技术相比，本发明公开的基于GNSS/INS深组合导航的双速率卡尔曼滤波方法具有以下优点：1、该方法考虑了系统状态误差的噪声，提高对状态量的更新频率，可以使系统状态估计的截断误差更小；2、在卫星信号不稳定、状态传播时间可能较长的情况下，充分利用了惯性导航的高频率输出数据，使卡尔曼滤波系统的状态一步预测误差大大减小；3、可以减小两种传感器因时间不同步而造成的系统误差，提高GNSS/INS组合导航系统的定位精度。
附图说明
图1为本发明公开的双速率卡尔曼滤波方法的流程图；
图2为实施例中卫星数据与惯性数据时间间隔示意图。
具体实施方式
本发明公开了一种基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，下面结合附图进一步阐述本发明。
一种基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，如图1所示，包括以下步骤：
(1)根据载体的初始位置、速率和姿态信息构建状态方程，初始化卡尔曼滤波的协方差误差矩阵P0、系统噪声协方差矩阵Q0、观测误差矩阵R0；
所述17维状态方程为：
其中3维参数为载体系到ECEF系的失准角，3维参数为ECEF系下的速度误差，3维参数为ECEF系下的位置误差，3维参数ba为加速度计偏置，3维参数bg为陀螺仪偏置，1维参数δtu为接收机的钟差，1维参数δtru为接收机的钟漂；上标T表示矩阵的转置。
初始化卡尔曼滤波的协方差误差矩阵P0、系统噪声协方差矩阵Q0、观测误差矩阵R0的确定参考Paul D.Groves撰写的第二版《Principle of GNSS，Inertial，and Multi-sensor Integrated Navigation System》。其中经过简化的Q0矩阵初始化可参照公式(14.82)，精确建模则为公式(14.80)，之后会随SAGE_HUSA自适应算法自动更新。观测误差矩阵R在计算过程中与R0一致，保持不变，具体为：
即R为对角阵，前四个对角元素为卫星导航器件标称的伪距误差均方值△ρGNSS 2，后四个对角元素为标称的伪距率误差均方值
P0同样为对角阵，对角元素与状态量相对应，分别为失准角、速度、位置、陀螺偏置、加计偏置、钟差及钟漂的不确定度平方值。不确定度由惯性及卫星器件决定或由实验得到与该器件相对应的经验值。
设惯性数据两次更新之间的时间间隔为τINS，卫星数据的更新时间间隔为τGNSS，两者之间存在整数倍倍数关系：MτINS＝τGNSS。因为惯性器件的输出频率一般大于卫星导航器件的输出频率，所以有M≥1。
第一次卡尔曼滤波以P0、Q0、R0进行计算；对于后续循环，P、Q矩阵随滤波改变，而R＝R0。
(2)进行M步步长的状态预测更新，得到先验状态量的预测值M为惯性数据和卫星数据更新频率之比；根据外推得到载体当前位置和速度信息。
参考Paul D.Groves撰写的第二版《Principle of GNSS，Inertial，and Multi-sensor Integrated Navigation System》，可得到17×17维的离散化状态转移矩阵在器件精度较高情况下，零偏不随时间变化。对于一般的卡尔曼滤波而言，是经度Lon、纬度Lat以及两次卡尔曼滤波之间时间间隔τKal的函数，可以表示为Φe(Lat,Lon,τKal)。对τKal是线性的。对于双速率卡尔曼滤波的状态预测，将惯性数据两次更新之间的时间间隔τINS作为状态预测的间隔时间，并采用每次惯导解算得到的经纬度，省略代表ECEF系的上标e，则有ΦS(Lat,Lon,τINS)。由此状态更新方程为：
其中k＝0,1…,M-1，为第k+1步的先验状态量预测值；为第k步的后验状态量预测值；ΦS,k为第k步更新时的离散化状态转移矩阵，为当前惯导解算得到的经度Lon、纬度Lat和惯性数据两次更新之间的时间间隔τINS的函数。
当对数据进行组合时，考虑到卫星数据与惯性数据不在同一时间域中，应通过时间标签将两者置于同一时间轴上。忽略数据由于传输而导致的延迟，并补偿卫星模块从接收到信号至计算出卫星结果的计算延迟后，假设到来的卫星数据所在时刻在第m次惯性数据之后，且距第m次惯性数据的时间差为△τm，如图2所示，图中1为惯性数据，2为卫星数据，该卫星数据所在时刻与该卫星数据之前最近一次惯导数据所在时刻的时间差为△τm，接收到卫星数据时第m+1次惯性数据尚未得到。选用ΦS′(Lat,Lon,△τm)进一步对状态量进行修正，得到卫星数据所在时间点上的状态量作为先验状态量的预测值修正方程为：
其中为修正前的先验状态量的预测值，为修正后的先验状态量的预测值，Φ′S,k为第k步的观测更新的状态修正转移矩阵，为当前惯导解算得到的经度Lon、纬度Lat、卫星数据所在时刻与该卫星数据之前最近一次惯导数据所在时刻的时间差△τm的函数。
根据外推得到载体在卫星数据所在的时间点的位置和速度信息，然后经转换送入组合导航滤波器。
惯性导航数据外推是利用当前惯导得到的速度、位置进行为时△τm的外推。假设之前时刻存在速度v和位置r，并且可知当前惯性解算得到的速度增量为△v，则外推过程即为：
v＝v+△v×△τm
r＝r+v×△τm
(3)对先验状态量进行修正，得到后验状态量的预测值根据外推得到载体当前位置和速度信息；
由于观测更新的时间周期为卫星数据的更新时间间隔τGNSS，所以取上一次观测更新时得到的经纬度，得观测更新的状态转移矩阵ΦO(Lat,Lon,τGNSS)。
深组合方案选用伪距/伪距率偏差构成矢量作为观测量，送入组合导航融合滤波器，生成系统误差并反馈给INS。可知INS推算伪距ρINS、伪距率与GNSS子系统输出的伪距ρGNSS、伪距率之差，与ECEF坐标系下的δx、δy、δz以及钟差δtu和钟漂δtru有如下关系：
观测量：
其中为δx δy δz的导数。
对应状态向量的H矩阵可以用下式表示：
其中，为对应n颗卫星到用户天线之间的视线矢量所构成的n×3维矩阵，n为选用的卫星数，一般为4颗，j指代第j颗卫星；Hρ2＝[1 0]n×2；Hρ3＝[01]n×2；和分别为对应第j颗卫星的伪距和伪距率测量误差。对于第j颗卫星，假设卫星位置为用户接收机位置为(xa,ya,za)，则能由下式计算得到，和同理。
由此得到更新方程为：
其中ΦO,k为第k步的观测更新的状态转移矩阵，为当前惯导解算得到的经度Lon、纬度Lat和卫星数据的更新时间τGNSS的函数；为第k步的卡尔曼滤波的后验协方差误差矩阵预测值；为第k+M步的卡尔曼滤波的先验协方差误差矩阵预测值；为第k+M步的卡尔曼滤波的后验协方差误差矩阵预测值；Qk为第k步的卡尔曼滤波的系统噪声协方差矩阵；Hk+M为第k+M步的状态向量的H矩阵；R为卡尔曼滤波的观测误差矩阵；(·)-1为矩阵求逆运算符；I为单位矩阵；Zk+M为第k+M步的卡尔曼滤波的观测值。
(4)对状态量的误差qk+M和系统误差协方差矩阵Qk+M进行自适应更新，并用后验状态量预测值对惯性导航结果进行补偿，得到载体位置、速度与姿态信息；
本发明采用SAGE_HUSA算法对状态量的误差qk+M和系统误差协方差矩阵Qk+M进行自适应更新，更新方程为：
其中b为遗忘因子，取值范围为(0.95,0.99)。
用后验状态量预测值对惯性导航结果进行补偿，包括如下步骤：
(4.1)根据和状态方程得到载体系到ECEF系的失准角ECEF系下的速度误差ECEF系下的位置误差加速度计偏置ba，陀螺仪偏置bg，接收机的钟差δtu和接收机的钟漂δtru；
(4.2)从载体系到ECEF坐标系的变换矩阵为可以得到补偿后的变换矩阵将ECEF坐标系下的速度、位置值减去和得到载体的速度位置值；将上一时刻加速度计偏置和陀螺仪偏置分别于ba和bg相加得到当前加速度计偏置和陀螺仪偏置。
此外，利用INS辅助的信息以及卫星位置/速度，计算载波和码对应伪距和伪距变化率，然后对伪距/伪距率求导，得到码相位/变化率和载波相位/变化率，进而推算出NCO控制量。
假设解算得到的速度为加速度为其中上标代表ECEF坐标系，下标为与卫星系统输出相区分，参照北京自动化控制设备研究所的薛涛著论文《基于多普勒辅助的惯性/卫星深组合技术研究》设定。根据文中式(10)和(12)：
可得到本地NCO产生的中心频率，然后再由加速度辅助公式，即式(13)和(15)进行计算得到增量，最后相加得到下一时刻载波频率产生的基准，即可起到对跟踪环路辅助的作用：
(5)补偿完成后更新用于下一轮卡尔曼滤波的状态误差估计，更新方程为：
每次闭环补偿完成后，按照上式更新用于下一轮卡尔曼滤波的一步预测中。
Claims (7)
1.一种基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，其特征在于，包括以下步骤：
(1)根据载体的初始位置、速率和姿态信息构建状态方程，初始化卡尔曼滤波的协方差误差矩阵P0、系统噪声协方差矩阵Q0、观测误差矩阵R0；
所述状态方程为：
其中为载体系到ECEF系的失准角，为ECEF系下的速度误差，和为ECEF系下的位置误差，ba为加速度计偏置，bg为陀螺仪偏置，δtu为接收机的钟差，δtru为接收机的钟漂；T表示矩阵的转置；
(2)进行M步步长的状态预测更新，得到先验状态量的预测值M为惯性数据和卫星数据更新频率之比；根据外推得到载体当前位置和速度信息；
(3)对先验状态量进行修正，得到后验状态量的预测值根据外推得到载体当前位置和速度信息；
(4)对状态量的误差qk+M和系统误差协方差矩阵Qk+M进行自适应更新，并用后验状态量预测值对惯性导航结果进行补偿，得到载体位置、速度与姿态信息；
(5)补偿完成后更新更新方程为：
<mrow>
<msubsup>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>+</mo>
</msubsup>
<mo>=</mo>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>1</mn>
<mo>&times;</mo>
<mn>15</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&delta;t</mi>
<mi>u</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&delta;t</mi>
<mrow>
<mi>r</mi>
<mi>u</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mo>+</mo>
<msub>
<mi>q</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<mo>.</mo>
</mrow>
2.根据权利要求1所述的基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，其特征在于，步骤(2)中状态更新方程为：
<mrow>
<msubsup>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mo>-</mo>
</msubsup>
<mo>=</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<msubsup>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
<mo>+</mo>
</msubsup>
</mrow>
其中k＝0,1…,M-1，为第k+1步的先验状态量预测值；为第k步的后验状态量预测值；ΦS,k为第k步更新时的离散化状态转移矩阵，为当前惯导解算得到的经度Lon、纬度Lat和惯性数据两次更新之间的时间间隔τINS的函数。
3.根据权利要求1所述的基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，其特征在于，步骤(3)中更新方程为：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>-</mo>
</msubsup>
<mo>=</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>O</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>P</mi>
<mi>k</mi>
<mo>+</mo>
</msubsup>
<mo>+</mo>
<mn>0.5</mn>
<mo>&times;</mo>
<msub>
<mi>Q</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
<msubsup>
<mi>&Phi;</mi>
<mrow>
<mi>O</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<mn>0.5</mn>
<mo>&times;</mo>
<msub>
<mi>Q</mi>
<mi>k</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<mo>=</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>-</mo>
</msubsup>
<msubsup>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mi>T</mi>
</msubsup>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>-</mo>
</msubsup>
<msubsup>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<mi>R</mi>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>+</mo>
</msubsup>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mi>I</mi>
<mo>-</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<msub>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>-</mo>
</msubsup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>+</mo>
</msubsup>
<mo>=</mo>
<msubsup>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>-</mo>
</msubsup>
<mo>+</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<msub>
<mi>Z</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
其中ΦO,k为第k步的观测更新的状态转移矩阵，为当前惯导解算得到的经度Lon、纬度Lat和卫星数据的更新时间τGNSS的函数；为第k步的卡尔曼滤波的后验协方差误差矩阵预测值；为第k+M步的卡尔曼滤波的先验协方差误差矩阵预测值；为第k+M步的卡尔曼滤波的后验协方差误差矩阵预测值；Qk为第k步的卡尔曼滤波的系统噪声协方差矩阵；Hk+M为第k+M步的状态向量的H矩阵；R为卡尔曼滤波的观测误差矩阵；(·)-1为矩阵求逆运算符；I为单位矩阵；Zk+M为第k+M步的卡尔曼滤波的观测值。
4.根据权利要求1所述的基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，其特征在于，步骤(4)中状态量的误差qk+M和系统误差协方差矩阵Qk+M进行自适应更新的方程为：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<mo>=</mo>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
<msub>
<mi>q</mi>
<mi>k</mi>
</msub>
<mo>+</mo>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>(</mo>
<msubsup>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>+</mo>
</msubsup>
<mo>-</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>O</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<msubsup>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
<mo>+</mo>
</msubsup>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Q</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<mo>=</mo>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
<msub>
<mi>Q</mi>
<mi>k</mi>
</msub>
<mo>+</mo>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>(</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<msub>
<mi>Z</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
</msub>
<msubsup>
<mi>Z</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mi>T</mi>
</msubsup>
<msubsup>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mi>T</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>M</mi>
</mrow>
<mo>+</mo>
</msubsup>
<mo>-</mo>
<msub>
<mi>&Phi;</mi>
<mrow>
<mi>O</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
</msub>
<msubsup>
<mi>P</mi>
<mi>k</mi>
<mo>+</mo>
</msubsup>
<msubsup>
<mi>&Phi;</mi>
<mrow>
<mi>O</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mi>T</mi>
</msubsup>
<mo>)</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mi>b</mi>
<mo>)</mo>
<mo>/</mo>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<msup>
<mi>b</mi>
<mrow>
<mi>k</mi>
<mo>/</mo>
<mi>M</mi>
</mrow>
</msup>
<mo>)</mo>
</mtd>
</mtr>
</mtable>
</mfenced>
其中b为遗忘因子。
5.根据权利要求1所述的基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，其特征在于，步骤(4)中用后验状态量预测值对惯性导航结果进行补偿，包括如下步骤：
(4.1)根据和状态方程得到载体系到ECEF系的失准角ECEF系下的速度误差ECEF系下的位置误差加速度计偏置ba，陀螺仪偏置bg，接收机的钟差δtu和接收机的钟漂δtru；
(4.2)从载体系到ECEF坐标系的变换矩阵为可以得到补偿后的变换矩阵将ECEF坐标系下的速度、位置值减去和得到载体的速度位置值；将上一时刻加速度计偏置和陀螺仪偏置分别于ba和bg相加得到当前加速度计偏置和陀螺仪偏置。
6.根据权利要求1所述的基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，其特征在于，步骤(2)还包括对先验状态量的预测值的进一步修正，修正方程为：
<mrow>
<msubsup>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mo>-</mo>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>&Phi;</mi>
<mrow>
<mi>S</mi>
<mo>,</mo>
<mi>k</mi>
</mrow>
<mo>&prime;</mo>
</msubsup>
<msubsup>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mrow>
<mo>&prime;</mo>
<mo>-</mo>
</mrow>
</msubsup>
</mrow>
其中为修正前的先验状态量的预测值，为修正后的先验状态量的预测值，Φ′S,k为第k步的观测更新的状态修正转移矩阵，为当前惯导解算得到的经度Lon、纬度Lat、卫星数据所在时刻与该卫星数据之前最近一次惯导数据所在时刻的时间差Δτm的函数。
7.根据权利要4所述的基于GNSS/INS深组合导航的双速率卡尔曼滤波方法，其特征在于，遗忘因子b的取值范围为(0.95,0.99)。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710811405.7A CN107643534B (zh) | 2017-09-11 | 2017-09-11 | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710811405.7A CN107643534B (zh) | 2017-09-11 | 2017-09-11 | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107643534A true CN107643534A (zh) | 2018-01-30 |
| CN107643534B CN107643534B (zh) | 2019-07-12 |
Family
ID=61110451
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710811405.7A Active CN107643534B (zh) | 2017-09-11 | 2017-09-11 | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107643534B (zh) |
Cited By (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108548535A (zh) * | 2018-03-13 | 2018-09-18 | 杨勇 | 低速gnss/mems组合导航系统的初始化方法 |
| CN108985373A (zh) * | 2018-07-12 | 2018-12-11 | 中国人民解放军陆军炮兵防空兵学院郑州校区 | 一种多传感器数据加权融合方法 |
| CN109443342A (zh) * | 2018-09-05 | 2019-03-08 | 中原工学院 | 新型自适应卡尔曼无人机姿态解算方法 |
| CN109596139A (zh) * | 2019-01-22 | 2019-04-09 | 中国电子科技集团公司第十三研究所 | 基于mems的车载导航方法 |
| CN110208843A (zh) * | 2019-05-21 | 2019-09-06 | 南京航空航天大学 | 一种基于增广伪距信息辅助的容错导航方法 |
| CN110568467A (zh) * | 2019-08-19 | 2019-12-13 | 北京自动化控制设备研究所 | 载波相位差分定位信息输出频率切换的设计方法 |
| CN110645979A (zh) * | 2019-09-27 | 2020-01-03 | 北京交通大学 | 基于gnss/ins/uwb组合的室内外无缝定位方法 |
| CN110986929A (zh) * | 2019-11-25 | 2020-04-10 | 四川航天系统工程研究所 | 导航与控制周期不同步的飞行控制方案的软件实现方法 |
| CN111175795A (zh) * | 2020-01-03 | 2020-05-19 | 暨南大学 | Gnss/ins组合导航系统的两步抗差滤波方法及系统 |
| CN111399022A (zh) * | 2020-04-08 | 2020-07-10 | 湖南格艾德电子科技有限公司 | 一种卫星导航与惯性导航超紧组合仿真定位的方法及终端 |
| CN112050807A (zh) * | 2020-08-03 | 2020-12-08 | 河北汉光重工有限责任公司 | 一种基于时间同步补偿的sins_gnss组合导航方法 |
| CN112229401A (zh) * | 2020-09-15 | 2021-01-15 | 北京菲斯罗克光电技术有限公司 | 适用于ins-gps伪距融合的量测信息同步外推方法及系统 |
| CN113009816A (zh) * | 2021-03-08 | 2021-06-22 | 北京信息科技大学 | 时间同步误差的确定方法及装置、存储介质及电子装置 |
| CN113203418A (zh) * | 2021-04-20 | 2021-08-03 | 同济大学 | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 |
| CN113449248A (zh) * | 2020-03-26 | 2021-09-28 | 太原理工大学 | 集成sins/gnss系统数据融合方法和装置 |
| CN113848579A (zh) * | 2021-11-29 | 2021-12-28 | 北京北斗华大科技有限公司 | Ins辅助gnss定位的粗差剔除方法及系统 |
| CN114459472A (zh) * | 2022-02-15 | 2022-05-10 | 上海海事大学 | 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 |
| CN115164886A (zh) * | 2022-07-22 | 2022-10-11 | 吉林大学 | 车载gnss/ins组合导航系统比例因子误差补偿方法 |
| CN116047567A (zh) * | 2023-04-03 | 2023-05-02 | 长沙金维信息技术有限公司 | 基于深度学习辅助的卫惯组合定位方法及导航方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101666868A (zh) * | 2009-09-30 | 2010-03-10 | 北京航空航天大学 | 一种基于sins/gps深组合数据融合的卫星信号矢量跟踪方法 |
| CN102297695A (zh) * | 2010-06-22 | 2011-12-28 | 中国船舶重工集团公司第七○七研究所 | 一种深组合导航系统中的卡尔曼滤波处理方法 |
| CN104392136A (zh) * | 2014-11-28 | 2015-03-04 | 东南大学 | 一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法 |
| CN106291645A (zh) * | 2016-07-19 | 2017-01-04 | 东南大学 | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 |
-
2017
- 2017-09-11 CN CN201710811405.7A patent/CN107643534B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101666868A (zh) * | 2009-09-30 | 2010-03-10 | 北京航空航天大学 | 一种基于sins/gps深组合数据融合的卫星信号矢量跟踪方法 |
| CN102297695A (zh) * | 2010-06-22 | 2011-12-28 | 中国船舶重工集团公司第七○七研究所 | 一种深组合导航系统中的卡尔曼滤波处理方法 |
| CN104392136A (zh) * | 2014-11-28 | 2015-03-04 | 东南大学 | 一种面向高动态非高斯模型鲁棒测量的高精度数据融合方法 |
| CN106291645A (zh) * | 2016-07-19 | 2017-01-04 | 东南大学 | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 |
Non-Patent Citations (3)
| Title |
|---|
| BINGBO CUI ET AL.: "Improved Cubature Kalman Filter for GNSS/INS Based on Transformation of Posterior Sigma-Points Error", 《IEEE TRANSACTIONS ON SIGNAL PROCESSING》 * |
| SONGLAI HAN ET AL.: "Integrated GPS/INS navigation system with dual-rate Kalman Filter", 《GPS SOLUTIONS》 * |
| XU YUAN ET AL.: "Distributed unbiased tightly-coupled INS/UWB human", 《中国惯性技术学报》 * |
Cited By (30)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108548535A (zh) * | 2018-03-13 | 2018-09-18 | 杨勇 | 低速gnss/mems组合导航系统的初始化方法 |
| CN108985373A (zh) * | 2018-07-12 | 2018-12-11 | 中国人民解放军陆军炮兵防空兵学院郑州校区 | 一种多传感器数据加权融合方法 |
| CN108985373B (zh) * | 2018-07-12 | 2021-09-14 | 中国人民解放军陆军炮兵防空兵学院郑州校区 | 一种多传感器数据加权融合方法 |
| CN109443342A (zh) * | 2018-09-05 | 2019-03-08 | 中原工学院 | 新型自适应卡尔曼无人机姿态解算方法 |
| CN109596139A (zh) * | 2019-01-22 | 2019-04-09 | 中国电子科技集团公司第十三研究所 | 基于mems的车载导航方法 |
| CN110208843A (zh) * | 2019-05-21 | 2019-09-06 | 南京航空航天大学 | 一种基于增广伪距信息辅助的容错导航方法 |
| CN110208843B (zh) * | 2019-05-21 | 2022-07-22 | 南京航空航天大学 | 一种基于增广伪距信息辅助的容错导航方法 |
| CN110568467A (zh) * | 2019-08-19 | 2019-12-13 | 北京自动化控制设备研究所 | 载波相位差分定位信息输出频率切换的设计方法 |
| CN110568467B (zh) * | 2019-08-19 | 2021-07-13 | 北京自动化控制设备研究所 | 载波相位差分定位信息输出频率切换的设计方法 |
| CN110645979B (zh) * | 2019-09-27 | 2021-09-21 | 北京交通大学 | 基于gnss/ins/uwb组合的室内外无缝定位方法 |
| CN110645979A (zh) * | 2019-09-27 | 2020-01-03 | 北京交通大学 | 基于gnss/ins/uwb组合的室内外无缝定位方法 |
| CN110986929A (zh) * | 2019-11-25 | 2020-04-10 | 四川航天系统工程研究所 | 导航与控制周期不同步的飞行控制方案的软件实现方法 |
| CN111175795B (zh) * | 2020-01-03 | 2023-05-26 | 暨南大学 | Gnss/ins组合导航系统的两步抗差滤波方法及系统 |
| CN111175795A (zh) * | 2020-01-03 | 2020-05-19 | 暨南大学 | Gnss/ins组合导航系统的两步抗差滤波方法及系统 |
| CN113449248A (zh) * | 2020-03-26 | 2021-09-28 | 太原理工大学 | 集成sins/gnss系统数据融合方法和装置 |
| CN113449248B (zh) * | 2020-03-26 | 2022-04-12 | 太原理工大学 | 集成sins/gnss系统数据融合方法和装置 |
| CN111399022A (zh) * | 2020-04-08 | 2020-07-10 | 湖南格艾德电子科技有限公司 | 一种卫星导航与惯性导航超紧组合仿真定位的方法及终端 |
| CN112050807A (zh) * | 2020-08-03 | 2020-12-08 | 河北汉光重工有限责任公司 | 一种基于时间同步补偿的sins_gnss组合导航方法 |
| CN112050807B (zh) * | 2020-08-03 | 2023-08-18 | 河北汉光重工有限责任公司 | 一种基于时间同步补偿的sins_gnss组合导航方法 |
| CN112229401A (zh) * | 2020-09-15 | 2021-01-15 | 北京菲斯罗克光电技术有限公司 | 适用于ins-gps伪距融合的量测信息同步外推方法及系统 |
| CN112229401B (zh) * | 2020-09-15 | 2022-08-05 | 北京菲斯罗克光电技术有限公司 | 适用于ins-gps伪距融合的量测信息同步外推方法及系统 |
| CN113009816A (zh) * | 2021-03-08 | 2021-06-22 | 北京信息科技大学 | 时间同步误差的确定方法及装置、存储介质及电子装置 |
| CN113009816B (zh) * | 2021-03-08 | 2022-06-21 | 北京信息科技大学 | 时间同步误差的确定方法及装置、存储介质及电子装置 |
| CN113203418A (zh) * | 2021-04-20 | 2021-08-03 | 同济大学 | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 |
| CN113848579B (zh) * | 2021-11-29 | 2022-03-08 | 北京北斗华大科技有限公司 | Ins辅助gnss定位的粗差剔除方法及系统 |
| CN113848579A (zh) * | 2021-11-29 | 2021-12-28 | 北京北斗华大科技有限公司 | Ins辅助gnss定位的粗差剔除方法及系统 |
| CN114459472A (zh) * | 2022-02-15 | 2022-05-10 | 上海海事大学 | 一种容积卡尔曼滤波器和离散灰色模型的组合导航方法 |
| CN115164886A (zh) * | 2022-07-22 | 2022-10-11 | 吉林大学 | 车载gnss/ins组合导航系统比例因子误差补偿方法 |
| CN115164886B (zh) * | 2022-07-22 | 2023-09-05 | 吉林大学 | 车载gnss/ins组合导航系统比例因子误差补偿方法 |
| CN116047567A (zh) * | 2023-04-03 | 2023-05-02 | 长沙金维信息技术有限公司 | 基于深度学习辅助的卫惯组合定位方法及导航方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107643534B (zh) | 2019-07-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107643534B (zh) | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 | |
| CN106291645B (zh) | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 | |
| US11105633B2 (en) | Navigation system utilizing yaw rate constraint during inertial dead reckoning | |
| Li et al. | A fast SINS initial alignment scheme for underwater vehicle applications | |
| US9791575B2 (en) | GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter | |
| US8600660B2 (en) | Multipath modeling for deep integration | |
| US7248964B2 (en) | System and method for using multiple aiding sensors in a deeply integrated navigation system | |
| Li et al. | A novel backtracking navigation scheme for autonomous underwater vehicles | |
| US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
| US20040150557A1 (en) | Inertial GPS navigation system with modified kalman filter | |
| CN108241161A (zh) | 用于载波范围模糊度估计的分布式卡尔曼滤波器架构 | |
| CN107690567A (zh) | 利用扩展卡尔曼滤波器用于对移动载体设备的航行进行追踪的方法 | |
| EP2957928B1 (en) | Method for using partially occluded images for navigation and positioning | |
| JP2012207919A (ja) | 異常値判定装置、測位装置、及びプログラム | |
| CN107677272A (zh) | 一种基于非线性信息滤波的auv协同导航方法 | |
| US20150153460A1 (en) | Sequential Estimation in a Real-Time Positioning or Navigation System Using Historical States | |
| CN108761512A (zh) | 一种弹载bds/sins深组合自适应ckf滤波方法 | |
| WO2013080183A9 (en) | A quasi tightly coupled gnss-ins integration process | |
| US20110137560A1 (en) | Method and system for latitude adaptive navigation quality estimation | |
| WO2005071431A1 (en) | Inertial gps navigation system with modified kalman filter | |
| Farrell et al. | GNSS/INS Integration | |
| Williamson et al. | A comparison of state space, range space, and carrier phase differential GPS/INS relative navigation | |
| Cho et al. | Modified RHKF filter for improved DR/GPS navigation against uncertain model dynamics | |
| EP3631515B1 (en) | Method for estimating a position of a mobile device using gnss signals | |
| KR101723751B1 (ko) | 위성체의 항법 제어 장치 및 방법 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |