CN111811512A - 基于联邦平滑的mpos离线组合估计方法和装置 - Google Patents

基于联邦平滑的mpos离线组合估计方法和装置 Download PDF

Info

Publication number
CN111811512A
CN111811512A CN202010489028.1A CN202010489028A CN111811512A CN 111811512 A CN111811512 A CN 111811512A CN 202010489028 A CN202010489028 A CN 202010489028A CN 111811512 A CN111811512 A CN 111811512A
Authority
CN
China
Prior art keywords
sub
estimation
filter
global
filters
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
Application number
CN202010489028.1A
Other languages
English (en)
Other versions
CN111811512B (zh
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.)
Beihang University
Original Assignee
Beihang University
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 Beihang University filed Critical Beihang University
Priority to CN202010489028.1A priority Critical patent/CN111811512B/zh
Publication of CN111811512A publication Critical patent/CN111811512A/zh
Application granted granted Critical
Publication of CN111811512B publication Critical patent/CN111811512B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本公开提供了基于联邦平滑的MPOS离线组合估计方法，针对机载对地观测离线成像对MPOS运动参数离线测量精度的高要求，将联邦滤波的分散化滤波与容错思想同平滑估计具有全局优化的特点相结合，设计前向滤波和后向平滑递推两个估计过程。在完成基于联邦滤波的前向滤波后，对子滤波器进行基于R‑T‑S固定区间平滑的后向递推，并对递推结果进行全局优化，得到全局的平滑估计结果。克服了传统联邦滤波方法仅利用当前时刻以及之前时刻的量测信息导致组合估计精度低的不足，能够利用所有观测信息，充分挖掘MPOS的潜在精度，从而提高了MPOS的离线组合估计精度。本公开还提出基于联邦平滑的MPOS离线组合估计装置。

Description

基于联邦平滑的MPOS离线组合估计方法和装置
技术领域
本公开涉及航空航天技术领域，具体而言，涉及基于联邦平滑的MPOS离线组合估计方法和装置。
背景技术
随着机载对地观测系统的不断发展，阵列式遥感载荷逐渐成为新的发展方向。与传统航空遥感载荷相比，其具有低成本、高效率、快速响应以及灵活机动的特点和优势。对于装备了微小型阵列式遥感载荷的航空遥感系统，需要对各载荷分布点的运动参数实现高精度测量。因此，迫切需要研制微小型位置与姿态测量系统(Micro Position andOrientation System,MPOS)，以实现微小型阵列式遥感载荷高精度成像的运动补偿。
MPOS主要由微小型惯性测量单元(Micro Inertial Measurement Unit，MIMU)、全球定位系统(Global Position System，GPS)接收机等传感器所组成，可以为载体提供位置、速度和姿态信息。MPOS的系统集成化和小型化可以大大减轻系统的体积和重量，同时降低成本，但是微小型惯性测量器件的采用不可避免的带来了测量精度差的问题，尤其是载体的航向角测量精度较低。因此，需要将其他航向测量传感器引入到MPOS中。目前，在航向角测量方面，主要有双天线GPS和磁强计两种测量航向角的传感器。双天线GPS具有测量精度高、误差不随时间累积以及使用简单方便的优点，但是载机机动、卫星星座构变化、电磁干扰等原因会导致GPS量测值出现异常，将直接降低MPOS组合估计的精度；磁强计虽然具有定姿完全自主、全天候服务的优点，但易受电磁干扰的影响。综上可以看出，单一的航向测量传感器不能满足稳定、可靠、高精度测量航向角的需求。因此，将这两种航向传感器同时引入到MPOS中，组成MIMU/双天线GPS/磁强计组合导航系统。然而，随着MPOS中传感器的增加，系统变的更加复杂。采用集中式卡尔曼滤波进行组合估计时，计算量明显增大，且存在传感器故障交叉污染的缺点。联邦滤波是一种并行的分散化滤波，其核心思想是利用并行运行的子滤波器来减小集中滤波器的计算负担，从而提高运算速度和滤波器的容错性能。目前，联邦滤波主要应用在多传感器实时信息融合方面。机载对地观测离线成像对MPOS运动参数的离线测量精度提出了很高的要求，而实时联邦滤波仅利用当前时刻以及以前时刻的量测信息对某一状态进行实时的估计，而无法利用当前时刻以后的所有量测信息，因此，联邦滤波的估计精度不能满足离线成像的需求。
发明内容
为了解决现有技术中的技术问题，本公开实施例提供了基于联邦平滑的MPOS离线组合估计方法和装置，能够提高MPOS的离线组合估计精度，从而满足机载对地观测离线成像对MPOS运动参数离线测量精度的要求。
第一方面，本公开实施例提供了基于联邦平滑的MPOS离线组合估计方法，所述方法包括：进行联邦滤波器结构设计，所述结构设计包括对两个子滤波器和一个主滤波器的设计；建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；存储当前时刻两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵、MIMU的捷联解算结果；由主滤波器估计出全局滤波解，并对两个子滤波器进行重置；用全局滤波解中的全局状态估计值校正当前时刻的MIMU的捷联解算结果直到联邦滤波结束；通过预先存储的两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵，对两个子滤波器分别进行后向平滑解算，得到两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵；通过获取的两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵进行全局融合计算，得到所有时刻的全局平滑估计结果；通过所有时刻的全局平滑估计结果计算出所有时刻的更高精度的MIMU的位置、速度和姿态。
第二方面，本公开实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述的方法的步骤。
第三方面，本公开实施例提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述的方法的步骤。
第四方面，本公开实施例提供了基于联邦平滑的MPOS离线组合估计装置，所述装置包括：设计模块，用于进行联邦滤波器结构设计，所述结构设计包括对两个子滤波器和一个主滤波器的设计；建立模块，用于建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；存储模块，用于存储当前时刻两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵、MIMU的捷联解算结果；估计与重置模块，用于由主滤波器估计出全局滤波解，并对两个子滤波器进行重置；校正模块，用于用全局滤波解中的全局状态估计值校正当前时刻的MIMU的捷联解算结果直到联邦滤波结束；解算模块，用于通过预先存储的两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵，对两个子滤波器分别进行后向平滑解算，得到两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵；全局融合计算模块，用于通过获取的两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵进行全局融合计算，得到所有时刻的全局平滑估计结果；估计计算模块，用于通过所有时刻的全局平滑估计结果计算出所有时刻的更高精度的MIMU的位置、速度和姿态。
本发明提供的基于联邦平滑的MPOS离线组合估计方法和装置，针对机载对地观测离线成像对MPOS运动参数离线测量精度的高要求，将联邦滤波的分散化滤波与容错思想同平滑估计具有全局优化的特点相结合，设计前向滤波和后向平滑递推两个估计过程。在完成基于联邦滤波的前向滤波后，对子滤波器进行基于R-T-S固定区间平滑的后向递推，并对递推结果进行全局优化，得到全局的平滑估计结果。克服了传统联邦滤波方法仅利用当前时刻以及之前时刻的量测信息导致组合估计精度低的不足，能够利用所有观测信息，充分挖掘MPOS的潜在精度，从而提高了MPOS的离线组合估计精度。
附图说明
为了更清楚地说明本公开实施例的技术方案，下面对实施例描述中所需要使用的附图作简单地介绍：
图1为本发明一个实施例中的基于联邦平滑的MPOS离线组合估计方法的步骤流程示意图；
图2为本发明另一实施例中的基于联邦平滑的MPOS离线组合估计方法的步骤流程示意图；
图3(a)为本发明一个实施例中的基于联邦平滑的MPOS离线组合估计方法中的前向联邦滤波结构图；
图3(b)为本发明一个实施例中的基于联邦平滑的MPOS离线组合估计方法中的R-T-S固定区间平滑后向递推解算结构图；
图4为本发明一个实施例中的基于联邦平滑的MPOS离线组合估计装置的结构示意图；
图5为本发明一个实施例中的基于联邦平滑的MPOS离线组合估计装置的硬件框图；
图6为本发明一个实施例中的计算机可读存储介质的示意图。
具体实施方式
下面结合附图和实施例对本申请进行进一步的详细介绍。
在下述介绍中，术语“第一”、“第二”仅为用于描述的目的，而不能理解为指示或暗示相对重要性。下述介绍提供了本公开的多个实施例，不同实施例之间可以替换或者合并组合，因此本申请也可认为包含所记载的相同和/或不同实施例的所有可能组合。因而，如果一个实施例包含特征A、B、C，另一个实施例包含特征B、D，那么本申请也应视为包括含有A、B、C、D的一个或多个所有其他可能的组合的实施例，尽管该实施例可能并未在以下内容中有明确的文字记载。
为了使本发明的目的、技术方案及优点更加清楚明白，以下通过实施例，并结合附图，对本发明基于联邦平滑的MPOS离线组合估计方法和装置的具体实施方式进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。
如图1所示，为一个实施例中的基于联邦平滑的MPOS离线组合估计方法的流程示意图，具体包括以下步骤：
步骤11，进行联邦滤波器结构设计，所述结构设计包括对两个子滤波器和一个主滤波器的设计。
步骤12，建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计。
步骤13，存储当前时刻两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵、MIMU的捷联解算结果。
步骤14，由主滤波器估计出全局滤波解，并对两个子滤波器进行重置。
步骤15，用全局滤波解中的全局状态估计值校正当前时刻的MIMU的捷联解算结果直到联邦滤波结束。
步骤16，通过预先存储的两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵，对两个子滤波器分别进行后向平滑解算，得到两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵。
步骤17，通过获取的两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵进行全局融合计算，得到所有时刻的全局平滑估计结果。
步骤18，通过所有时刻的全局平滑估计结果计算出所有时刻的更高精度的MIMU的位置、速度和姿态。
在本实施例中，针对机载对地观测离线成像对MPOS运动参数离线测量精度的高要求，将联邦滤波的分散化滤波与容错思想同平滑估计具有全局优化的特点相结合，设计前向滤波和后向平滑递推两个估计过程。在完成基于联邦滤波的前向滤波后，对子滤波器进行基于R-T-S固定区间平滑的后向递推，并对递推结果进行全局优化，得到全局的平滑估计结果。克服了传统联邦滤波方法仅利用当前时刻以及之前时刻的量测信息导致组合估计精度低的不足，能够利用所有观测信息，充分挖掘MPOS的潜在精度，从而提高了MPOS的离线组合估计精度。
为了更加清晰与准确地理解与应用本公开所涉及的基于联邦平滑的MPOS离线组合估计方法，进行以下示例。需要说明的是，本公开所保护的范围不限于以下示例。
如图2所示，为本发明另一实施例中的基于联邦平滑的MPOS离线组合估计方法的步骤流程示意图；图3(a)为本发明一个实施例中的基于联邦平滑的MPOS离线组合估计方法中的前向联邦滤波结构图；图3(b)为本发明一个实施例中的基于联邦平滑的MPOS离线组合估计方法中的R-T-S固定区间平滑后向递推解算结构图。
具体的，结合图2-3(a)-3(b)所示，本发明的具体方法实施如下：
第一，进行联邦滤波器的结构设计，包括两个子滤波器和一个主滤波器。
具体的，将MIMU看作公共参考系统，进行联邦滤波器的结构设计，包括两个子滤波器和一个主滤波器，结构图如图3(a)所示。MIMU和双天线GPS构成子滤波器1，并采用MIMU与双天线GPS的速度、位置和航向角之差作为子滤波器1的观测量；MIMU和磁强计构成子滤波器2，并采用MIMU与磁强计解算的航向角之差作为子滤波器2的观测量。
第二，建立两个子滤波器的数学模型并分别进行卡尔曼滤波，同时存储当前时刻两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵、MIMU的捷联解算结果。
具体的，根据第一中设计的联邦滤波器的结构，建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；同时存储当前时刻两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、MIMU的捷联解算结果，具体步骤如下：
1)建立两个子滤波器的数学模型
相关参考坐标系的定义包括：记i为地心惯性坐标系；e为地球坐标系；n为导航坐标系，导航坐标系为东北天地理坐标系；b为右前上载体坐标系。
①建立子滤波器1的数学模型
a)状态方程
子滤波器1的状态变量选为15维状态变量：
Figure BDA0002520204810000071
其中，φE 1、φN 1和φU 1分别表示子滤波器1中MIMU的东向失准角、北向失准角和天向失准角；
Figure BDA0002520204810000081
和
Figure BDA0002520204810000082
分别为子滤波器1中MIMU的东向速度误差、北向速度误差和天向速度误差；δL1、δλ1和δH1分别为子滤波器1中MIMU的纬度误差、经度误差和高度误差；
Figure BDA0002520204810000083
和
Figure BDA0002520204810000084
分别为子滤波器1中MIMUx轴、y轴和z轴的陀螺仪随机常值漂移；
Figure BDA0002520204810000085
和
Figure BDA0002520204810000086
分别为子滤波器1中MIMUx轴、y轴和z轴的加速度计常值偏置。
子滤波器1的状态方程如下：
Figure BDA0002520204810000087
其中，F1、G1和ω1分别为子滤波器1的状态转移矩阵、系统噪声驱动阵和系统噪声向量；系统噪声
Figure BDA0002520204810000088
为零均值高斯白噪声，
Figure BDA0002520204810000089
和
Figure BDA00025202048100000810
分别为子滤波器1中MIMU陀螺仪和加速度计的三轴随机误差，ω1的方差阵为Q1，Q1的取值由MIMU陀螺仪和加速度计的噪声水平决定。
根据对子滤波器1中状态变量的选取可知，子滤波器1中的误差方程包括姿态误差方程、速度误差方程、位置误差方程和惯性仪表随机常值误差方程，分别为：
Figure BDA00025202048100000811
Figure BDA00025202048100000812
Figure BDA00025202048100000813
Figure BDA00025202048100000814
其中，
Figure BDA00025202048100000815
为子滤波器1中MIMU的姿态误差角；
Figure BDA00025202048100000816
为导航坐标系相对惯性坐标系的转动角速度在导航坐标系下的表示，
Figure BDA00025202048100000817
为
Figure BDA00025202048100000818
的计算误差；
Figure BDA00025202048100000819
为载体坐标系到导航坐标系的转换矩阵；
Figure BDA0002520204810000091
为子滤波器1中MIMU的速度误差，Vn＝[VE VN VU]T，VE、VN和VU分别为MIMU在导航坐标系下的东向速度、北向速度和天向速度；fn＝[fE fN fU]T，fE、fN和fU分别为MIMU在导航坐标系中的东向比力、北向比力和天向比力；
Figure BDA0002520204810000092
为地球坐标系相对惯性坐标系的旋转角速度在导航坐标系下的投影，
Figure BDA0002520204810000093
为导航坐标系相对地球坐标系的旋转角速度在导航坐标系下的投影，
Figure BDA0002520204810000094
和
Figure BDA0002520204810000095
分别为
Figure BDA0002520204810000096
和
Figure BDA0002520204810000097
的计算误差；L、λ、H和δL1、δλ1、δH1分别为子滤波器1中MIMU的纬度、经度、高度和纬度误差、经度误差、高度误差；RM和RN分别为沿子午圈和卯酉圈的主曲率半径，且
Figure BDA0002520204810000098
Figure BDA0002520204810000099
Figure BDA00025202048100000910
和
Figure BDA00025202048100000911
分别为子滤波器1中MIMU在载体坐标系x轴、y轴和z轴陀螺常值漂移；
Figure BDA00025202048100000912
Figure BDA00025202048100000913
和
Figure BDA00025202048100000914
分别为子滤波器1中MIMU在载体坐标系x轴、y轴和z轴加计常值偏置。根据上述误差方程确定的F1和G1分别为：
Figure BDA00025202048100000915
Figure BDA00025202048100000916
其中，F11、F12、F13、F21、F22、F23、F31和F32如下式所示。
Figure BDA00025202048100000917
Figure BDA0002520204810000101
Figure BDA0002520204810000102
Figure BDA0002520204810000103
Figure BDA0002520204810000104
Figure BDA0002520204810000105
Figure BDA0002520204810000111
Figure BDA0002520204810000112
其中，T为滤波周期。
b)量测方程
子滤波器1的量测方程为：
z1＝H1x1+v1
其中，子滤波器1的观测量为：
Figure BDA0002520204810000113
δVE′、δVN′和δVU′分别为MINU与双天线GPS的东向速度、北向速度和天向速度的差值，δL'、δλ'、δH'和
Figure BDA0002520204810000114
分别为MIMU与双天线GPS的经度、纬度、高度和航向角的差值；量测噪声
Figure BDA0002520204810000115
Figure BDA0002520204810000116
Figure BDA0002520204810000117
vδL'、vδλ'、vδH'和
Figure BDA0002520204810000118
分别为双天线GPS的东向速度、北向速度、天向速度、经度、纬度、高度和航向角的量测噪声，其方差阵R1根据双天线GPS的速度、位置和航向角的噪声水平选取，量测矩阵H1表示如下：
Figure BDA0002520204810000119
令载体坐标系到导航坐标系的转换矩阵
Figure BDA00025202048100001110
记Tlm为矩阵T中第l行、第m列的元素，l＝1,2,3，m＝1,2,3；则上式中H11和H12的表达式为：
Figure BDA00025202048100001111
Figure BDA0002520204810000121
②建立子滤波器2的数学模型
a)状态方程
子滤波器2的状态变量也选为15维状态变量：
Figure BDA0002520204810000122
其中，φE 2、φN 2和φU 2分别表示子滤波器2中MIMU的东向失准角、北向失准角和天向失准角；
Figure BDA0002520204810000123
和
Figure BDA0002520204810000124
分别为子滤波器2中MIMU的东向速度误差、北向速度误差和天向速度误差；δL2、δλ2和δH2分别为子滤波器2中MIMU的纬度误差、经度误差和高度误差；
Figure BDA0002520204810000125
和
Figure BDA0002520204810000126
分别为子滤波器2中MIMU x轴、y轴和z轴的陀螺仪随机常值漂移；
Figure BDA0002520204810000127
和
Figure BDA0002520204810000128
分别为子滤波器2中MIMUx轴、y轴和z轴的加速度计常值偏置。
子滤波器2的状态方程如下：
Figure BDA0002520204810000129
其中，F2、G2和ω2分别为子滤波器2的状态转移矩阵、系统噪声驱动矩阵和系统噪声向量；F2和G2的选取与子滤波器1中的F1和G1相同；系统噪声
Figure BDA00025202048100001210
为零均值高斯白噪声，
Figure BDA00025202048100001211
和
Figure BDA00025202048100001212
分分别为子滤波器2中MIMU在x轴、y轴和z轴陀螺仪的随机误差，
Figure BDA00025202048100001213
Figure BDA00025202048100001214
分别为子滤波器2中MIMU加速度计的三轴随机误差；ω2的方差阵为Q2，Q2的取值由MIMU陀螺仪和加速度计的噪声水平决定。
b)量测方程
子滤波器2的量测方程为：
z2＝H2x2+v2
其中，子滤波器2的系统观测量为：
Figure BDA00025202048100001215
Figure BDA00025202048100001216
为MIMU捷联解算与磁强计解算出的航向角的差值；系统量测噪声
Figure BDA00025202048100001217
Figure BDA00025202048100001218
为磁强计解算出的航向角的量测噪声，其方差阵R2由磁强计的测量精度决定；量测矩阵H2为：
Figure BDA0002520204810000131
2)根据子滤波器1和子滤波器2的数学模型分别进行卡尔曼滤波来求解各自的局部估计值，并将子滤波器1和子滤波器2当前时刻的局部状态估计值分别记为
Figure BDA0002520204810000132
和
Figure BDA0002520204810000133
局部状态估计协方差阵分别记为P1(k)和P2(k)、局部状态一步预测分别记为
Figure BDA0002520204810000134
和
Figure BDA0002520204810000135
局部状态一步预测协方差阵分别记为P1 -(k)和P2 -(k)。
3)存储当前时刻两个子滤波器的状态转移矩阵F1(k)和F2(k)、局部状态估计值
Figure BDA0002520204810000136
和
Figure BDA0002520204810000137
局部状态估计协方差阵P1(k)和P2(k)、局部状态一步预测
Figure BDA0002520204810000138
和
Figure BDA0002520204810000139
局部状态一步预测协方差阵P1 -(k)和P2 -(k)、MIMU的捷联解算结果，当前时刻MIMU的捷联解算结果包括导航坐标系下载体的航向角ψJ(k)、俯仰角θJ(k)、横滚角γJ(k)、东向速度
Figure BDA00025202048100001310
北向速度
Figure BDA00025202048100001311
天向速度
Figure BDA00025202048100001312
经度LJ(k)、纬度λJ(k)和高度HJ(k)。
第三，由主滤波器估计出全局滤波解，并对两个子滤波器进行重置；并用全局滤波解中的全局状态估计值校正当前时刻的MIMU的捷联解算结果。
具体的，由主滤波器估计出全局滤波解，并对两个子滤波器进行重置；并用全局滤波解中的全局状态估计值校正当前时刻的MIMU的捷联解算结果，具体步骤为：
1)将各子滤波器中当前时刻的局部状态估计值
Figure BDA00025202048100001313
和
Figure BDA00025202048100001314
局部状态估计协方差阵P1(k)和P2(k)输入到主滤波器中按照下式进行融合，即可得到当前时刻的全局滤波解：
Figure BDA00025202048100001315
其中，
Figure BDA00025202048100001316
表示当前时刻的全局状态估计值，
Figure BDA00025202048100001317
Figure BDA0002520204810000141
Figure BDA0002520204810000142
和
Figure BDA0002520204810000143
为当前时刻全局状态估计
Figure BDA0002520204810000144
中的东向失准角、北向失准角和天向失准角；
Figure BDA0002520204810000145
和
Figure BDA0002520204810000146
分别为当前时刻全局状态估计值
Figure BDA0002520204810000147
中的东向速度误差、北向速度误差和天向速度误差；δLf(k)、δλf(k)和δHf(k)分别为当前时刻全局状态估计值
Figure BDA0002520204810000148
中的纬度、经度和高度误差。
Figure BDA0002520204810000149
和
Figure BDA00025202048100001410
分别为当前时刻全局状态估计值
Figure BDA00025202048100001411
中的x轴、y轴和z轴的陀螺常值漂移；
Figure BDA00025202048100001412
和
Figure BDA00025202048100001413
分别为当前时刻全局状态估计值
Figure BDA00025202048100001414
中的x轴、y轴和z轴的加计常值偏置；Pg(k)表示当前时刻的全局状态估计协方差阵。
2)利用当前时刻的全局滤波解对两个子滤波器当前时刻的局部状态估计值和局部估计协方差阵进行重置，如下式所示：
Figure BDA00025202048100001415
Figure BDA00025202048100001416
Figure BDA00025202048100001417
3)用当前时刻全局滤波解中的的全局状态估计值
Figure BDA00025202048100001418
校正当前时刻的MIMU的位置、速度和姿态。校正方法如下所示：
①速度修正
Figure BDA00025202048100001419
其中，VE(k)、VN(k)和VU(k)分别为当前时刻滤波修正后MIMU的东向速度、北向速度和天向速度；
Figure BDA00025202048100001420
和
Figure BDA00025202048100001421
分别为当前时刻MIMU捷联解算得到的东向速度、北向速度和天向速度；
Figure BDA00025202048100001422
和
Figure BDA00025202048100001423
分别为当前时刻全局状态估计值
Figure BDA0002520204810000151
中的东向速度误差、北向速度误差和天向速度误差；
②位置修正
Figure BDA0002520204810000152
其中，L(k)、λ(k)和H(k)分别为当前时刻滤波修正后MIMU的纬度、经度和高度；LJ(k)、λJ(k)和HJ(k)分别为当前时刻MIMU捷联解算得到的纬度、经度和高度；δLf(k)、δλf(k)和δHf(k)分别为当前时刻全局状态估计值
Figure BDA0002520204810000153
中的纬度误差、经度误差和高度误差。
③姿态修正
计算当前时刻的MIMU导航坐标系n与计算导航坐标系n′间的转换矩阵
Figure BDA0002520204810000154
Figure BDA0002520204810000155
其中，
Figure BDA0002520204810000156
和
Figure BDA0002520204810000157
为当前时刻全局状态估计
Figure BDA0002520204810000158
中的东向失准角、北向失准角和天向失准角。
修正后的当前时刻的转换矩阵
Figure BDA0002520204810000159
为：
Figure BDA00025202048100001510
其中，
Figure BDA00025202048100001511
为当前时刻MIMU捷联解算得到的姿态矩阵；
利用修正后的转换矩阵
Figure BDA00025202048100001512
重新计算当前时刻MIMU的航向角、俯仰角和横滚角；将
Figure BDA00025202048100001513
记为：
Figure BDA00025202048100001514
其中，Tlm(k)为矩阵
Figure BDA0002520204810000161
中第l行、第m列的元素，l＝1,2,3，m＝1,2,3；则MIMU当前时刻的航向角ψ(k)、俯仰角θ(k)和横滚角γ(k)的主值，即ψ主(k)、θ主(k)和γ主(k)分别为：
Figure BDA0002520204810000162
θ主(k)＝arcsin(T32(k))
Figure BDA0002520204810000163
航向角ψ(k)、俯仰角θ(k)和横滚角γ(k)的取值范围分别定义为[0，2π]、
Figure BDA0002520204810000164
[-π，+π]；那么，ψ(k)、θ(k)和γ(k)的真值可由下式确定：
Figure BDA0002520204810000165
θ(k)＝θ主(k)
Figure BDA0002520204810000166
ψ(k)、θ(k)和γ(k)即为修正后的当前时刻MIMU的航向角、俯仰角和横滚角。
第四，不断重复第二和第三直到联邦滤波结束。
不断重复上述第二和第三直到联邦滤波结束，具体步骤如下：
1)当前时刻k＝k+1(k的起始值为1)；
2)若k<N或k＝N(N为MPOS进行组合估计的时间段的最后时刻)，则重复第二和第三；若k>N，则停止，表示联邦滤波过程结束。
第五，对两个子滤波器分别进行后向平滑解算。
利用第二中存储的信息，对两个子滤波器分别进行后向平滑解算，得到两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵，结构图如图3(b)所示，具体步骤如下：
以子滤波器1为例：
1)平滑初始化：
Figure BDA0002520204810000171
P1 S(N)＝P1(N)(上标S用于标记后向平滑值，N为MPOS进行组合估计的时间段的最后时刻)；
Figure BDA0002520204810000172
为N时刻子滤波器1的平滑估计值，P1 S(N)为N时刻子滤波器1的平滑估计协方差阵；
Figure BDA0002520204810000173
为N时刻子滤波器1的局部状态估计值，P1(N)为N时刻子滤波器1的局部估计协方差阵；
2)对于k＝N-1,…,2,1,利用联邦滤波存储的信息F1(k)、
Figure BDA0002520204810000174
P1(k)、
Figure BDA0002520204810000175
和P1 -(k)进行R-T-S固定区间平滑后向平滑递推解算，得到子滤波器1所有时刻的平滑估计值，递推公式如下：
Figure BDA0002520204810000176
其中，
Figure BDA0002520204810000177
为k时刻子滤波器1的平滑增益矩阵，
Figure BDA0002520204810000178
和
Figure BDA0002520204810000179
分别为k时刻和k+1时刻子滤波器1的平滑估计值，P1 S(k)和P1 S(k+1)分别为k时刻和k+1时刻子滤波器1的平滑估计协方差阵。
子滤波器2的后向平滑解算方法与子滤波器1相同，递推解算后得到子滤波器2N时刻的平滑估计值
Figure BDA00025202048100001710
平滑估计协方差阵
Figure BDA00025202048100001711
以及其它时刻的平滑估计值
Figure BDA00025202048100001712
平滑估计协方差阵
Figure BDA00025202048100001713
第六，进行全局融合计算并校正MIMU的位置、速度和姿态。
具体的，利用第五中得到的结果进行全局融合计算，得到所有时刻的全局平滑估计结果；并利用该结果计算出所有时刻的更加准确的MIMU的位置、速度和姿态，具体步骤为：
1)将两个子滤波器的平滑估计值
Figure BDA00025202048100001714
和
Figure BDA00025202048100001715
平滑估计协方差阵P1 S(k)和
Figure BDA00025202048100001716
按照下式进行全局融合计算，即可得到所有时刻的全局平滑估计结果：
Figure BDA0002520204810000181
其中，
Figure BDA0002520204810000182
为k时刻的全局平滑估计值，
Figure BDA0002520204810000183
Figure BDA0002520204810000184
Figure BDA0002520204810000185
和
Figure BDA0002520204810000186
为k时刻全局平滑估计值
Figure BDA0002520204810000187
中的东向失准角、北向失准角和天向失准角；
Figure BDA0002520204810000188
和
Figure BDA0002520204810000189
分别为k时刻的全局平滑估计值
Figure BDA00025202048100001810
中的东向速度误差、北向速度误差和天向速度误差；δLS(k)、δλS(k)和δHS(k)分别为k时刻全局平滑估计值
Figure BDA00025202048100001811
中的纬度误差、经度误差和高度误差；
Figure BDA00025202048100001812
和
Figure BDA00025202048100001813
分别为k时刻全局平滑估计值
Figure BDA00025202048100001814
中的x轴、y轴和z轴的陀螺常值漂移；
Figure BDA00025202048100001815
和
Figure BDA00025202048100001816
分别为k时刻全局平滑估计值
Figure BDA00025202048100001817
中的x轴、y轴和z轴的加计常值偏置；
Figure BDA00025202048100001818
为融合后的k时刻的全局平滑估计协方差阵。
2)利用全局平滑估计值
Figure BDA00025202048100001819
计算出所有时刻的更加准确的MIMU的位置、速度和姿态。具体的校正方法如下：
①速度修正
Figure BDA00025202048100001820
其中，
Figure BDA00025202048100001821
和
Figure BDA00025202048100001822
分别为平滑修正后k时刻的MIMU的东向速度、北向速度和天向速度；
Figure BDA00025202048100001823
和
Figure BDA00025202048100001824
分别为k时刻MIMU捷联解算得到的东向速度、北向速度和天向速度；
Figure BDA00025202048100001825
和
Figure BDA00025202048100001826
分别为k时刻的全局平滑估计值
Figure BDA00025202048100001827
中的东向速度误差、北向速度误差和天向速度误差。
②位置修正
Figure BDA0002520204810000191
其中，LS(k)、λS(k)和HS(k)分别为平滑修正后k时刻的MIMU纬度、经度和高度；LJ(k)、λJ(k)和HJ(k)分别为k时刻MIMU捷联解算得到的纬度、经度和高度；δLS(k)、δλS(k)和δHS(k)分别为k时刻全局平滑估计值
Figure BDA0002520204810000192
中的纬度误差、经度误差和高度误差。
③姿态修正
计算k时刻MIMU导航坐标系n与计算导航坐标系n′间的转换矩阵
Figure BDA0002520204810000193
Figure BDA0002520204810000194
其中，
Figure BDA0002520204810000195
和
Figure BDA0002520204810000196
为k时刻全局平滑估计值
Figure BDA0002520204810000197
中的东向失准角、北向失准角和天向失准角。
修正后的转换矩阵
Figure BDA0002520204810000198
为：
Figure BDA0002520204810000199
其中，
Figure BDA00025202048100001910
为k时刻MIMU进行捷联解算后得到的姿态矩阵；
利用修正后的转换矩阵
Figure BDA00025202048100001911
计算k时刻MIMU的航向角、俯仰角和横滚角，将
Figure BDA00025202048100001912
记为：
Figure BDA00025202048100001913
其中，
Figure BDA00025202048100001914
为矩阵
Figure BDA00025202048100001915
中第l行、第m列的元素，l＝1,2,3，m＝1,2,3；则MIMU在k时刻的航向角ψS(k)、俯仰角θS(k)和横滚角γS(k)的主值，即
Figure BDA00025202048100001916
和
Figure BDA00025202048100001917
分别为：
Figure BDA0002520204810000201
航向角ψS(k)、俯仰角θS(k)和横滚角γS(k)的取值范围分别定义为[0，2π]、
Figure BDA0002520204810000202
[-π，+π]；那么，ψS(k)、θS(k)和γS(k)的真值可由下式确定：
Figure BDA0002520204810000203
Figure BDA0002520204810000204
Figure BDA0002520204810000205
LS(k)、λS(k)、HS(k)、
Figure BDA0002520204810000206
ψS(k)、θS(k)和γS(k)(k＝N,…,2,1)即为平滑修正后所有时刻的更加准确的MIMU的经度、纬度、高度、东向速度、北向速度、天向速度、航向角、俯仰角和横滚角。
基于同一发明构思，还提供了基于联邦平滑的MPOS离线组合估计装置。由于此装置解决问题的原理与前述基于联邦平滑的MPOS离线组合估计方法相似，因此，该装置的实施可以按照前述方法的具体步骤实现，重复之处不再赘述。
如图4所示，为一个实施例中的基于联邦平滑的MPOS离线组合估计装置的结构示意图。该基于联邦平滑的MPOS离线组合估计装置10包括：设计模块100、建立模块200、存储模块300、估计与重置模块400、校正模块500、解算模块600、全局融合计算模块700和估计计算模块800。
其中，设计模块100用于进行联邦滤波器结构设计，所述结构设计包括对两个子滤波器和一个主滤波器的设计；建立模块200用于建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；存储模块300用于存储当前时刻两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵、MIMU的捷联解算结果；估计与重置模块400用于由主滤波器估计出全局滤波解，并对两个子滤波器进行重置；校正模块500用于用全局滤波解中的全局状态估计值校正当前时刻的MIMU的捷联解算结果直到联邦滤波结束；解算模块600用于通过预先存储的两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵，对两个子滤波器分别进行后向平滑解算，得到两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵；全局融合计算模块700用于通过获取的两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵进行全局融合计算，得到所有时刻的全局平滑估计结果；估计计算模块800，用于通过所有时刻的全局平滑估计结果计算出所有时刻的更高精度的MIMU的位置、速度和姿态。
图5是图示根据本公开的实施例的基于联邦平滑的MPOS离线组合估计装置的硬件框图。如图5所示，根据本公开实施例的基于联邦平滑的MPOS离线组合估计装置50包括存储器501和处理器502。基于联邦平滑的MPOS离线组合估计装置50中的各组件通过总线系统和/或其它形式的连接机构(未示出)互连。
存储器501用于存储非暂时性计算机可读指令。具体地，存储器501可以包括一个或多个计算机程序产品，计算机程序产品可以包括各种形式的计算机可读存储介质，例如易失性存储器和/或非易失性存储器。易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。
处理器502可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其它形式的处理单元，并且可以控制基于联邦平滑的MPOS离线组合估计装置50中的其它组件以执行期望的功能。在本公开的一个实施例中，所述处理器502用于运行存储器501中存储的计算机可读指令，使得基于联邦平滑的MPOS离线组合估计装置50执行上述基于联邦平滑的MPOS离线组合估计方法。基于联邦平滑的MPOS离线组合估计装置与上述基于联邦平滑的MPOS离线组合估计方法描述的实施例相同，在此将省略其重复描述。
图6是图示根据本公开的实施例的计算机可读存储介质的示意图。如图6所示，根据本公开实施例的计算机可读存储介质600其上存储有非暂时性计算机可读指令601。当所述非暂时性计算机可读指令601由处理器运行时，执行参照上述描述的根据本公开实施例的基于联邦平滑的MPOS离线组合估计方法。
以上，根据本公开实施例的基于联邦平滑的MPOS离线组合估计方法和装置，以及计算机可读存储介质，能够提高MPOS的离线组合估计精度，从而满足机载对地观测离线成像对MPOS运动参数离线测量精度的要求的有益效果。
以上结合具体实施例描述了本公开的基本原理，但是，需要指出的是，在本公开中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本公开的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本公开为必须采用上述具体的细节来实现。
本公开中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。
另外，如在此使用的，在以“至少一个”开始的项的列举中使用的“或”指示分离的列举，以便例如“A、B或C的至少一个”的列举意味着A或B或C，或AB或AC或BC，或ABC(即A和B和C)。此外，措辞“示例的”不意味着描述的例子是优选的或者比其他例子更好。
还需要指出的是，在本公开的系统和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本公开的等效方案。
可以不脱离由所附权利要求定义的教导的技术而进行对在此所述的技术的各种改变、替换和更改。此外，本公开的权利要求的范围不限于以上所述的处理、机器、制造、事件的组成、手段、方法和动作的具体方面。可以利用与在此所述的相应方面进行基本相同的功能或者实现基本相同的结果的当前存在的或者稍后要开发的处理、机器、制造、事件的组成、手段、方法或动作。因而，所附权利要求包括在其范围内的这样的处理、机器、制造、事件的组成、手段、方法或动作。
提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本公开。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本公开的范围。因此，本公开不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本公开的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。

Claims (11)

1.基于联邦平滑的MPOS离线组合估计方法，其特征在于，所述方法包括：
进行联邦滤波器结构设计，所述结构设计包括对两个子滤波器和一个主滤波器的设计；
建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；
存储当前时刻两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵、MIMU的捷联解算结果；
由主滤波器估计出全局滤波解，并对两个子滤波器进行重置；
用全局滤波解中的全局状态估计值校正当前时刻的MIMU的捷联解算结果直到联邦滤波结束；
通过预先存储的两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵，对两个子滤波器分别进行后向平滑解算，得到两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵；
通过获取的两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵进行全局融合计算，得到所有时刻的全局平滑估计结果；
通过所有时刻的全局平滑估计结果计算出所有时刻的更高精度的MIMU的位置、速度和姿态。
2.根据权利要求1所述的基于联邦平滑的MPOS离线组合估计方法，其特征在于，所述进行联邦滤波器结构设计包括：
MIMU和双天线GPS构成第一子滤波器，并采用MIMU与双天线GPS的速度、位置和航向角之差作为第一子滤波器的观测量；
MIMU和磁强计构成第二子滤波器，并采用MIMU与磁强计解算的航向角之差作为第二子滤波器的观测量。
3.根据权利要求1所述的基于联邦平滑的MPOS离线组合估计方法，其特征在于，所述建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计包括：
建立第一子滤波器和第二子滤波器的数学模型；
根据第一子滤波器和第二子滤波器的数学模型分别进行卡尔曼滤波来求解各自的局部估计值，并将第一子滤波器和第二子滤波器当前时刻的局部状态估计值分别记为
Figure FDA0002520204800000021
和
Figure FDA0002520204800000022
局部状态估计协方差阵分别记为P1(k)和P2(k)、局部状态一步预测分别记为
Figure FDA0002520204800000023
和
Figure FDA0002520204800000024
局部状态一步预测协方差阵分别记为P1 -(k)和P2 -(k)。
4.根据权利要求1所述的基于联邦平滑的MPOS离线组合估计方法，其特征在于，所述存储当前时刻两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵、MIMU的捷联解算结果包括：
存储当前时刻两个子滤波器的状态转移矩阵F1(k)和F2(k)、局部状态估计值
Figure FDA0002520204800000025
和
Figure FDA0002520204800000026
局部状态估计协方差阵P1(k)和P2(k)、局部状态一步预测
Figure FDA0002520204800000027
和
Figure FDA0002520204800000028
局部状态一步预测协方差阵P1 -(k)和P2 -(k)、MIMU的捷联解算结果，当前时刻MIMU的捷联解算结果包括导航坐标系下载体的航向角ψJ(k)、俯仰角θJ(k)、横滚角γJ(k)、东向速度
Figure FDA0002520204800000029
北向速度
Figure FDA00025202048000000210
天向速度
Figure FDA00025202048000000211
经度LJ(k)、纬度λJ(k)和高度HJ(k)。
5.根据权利要求1所述的基于联邦平滑的MPOS离线组合估计方法，其特征在于，所述由主滤波器估计出全局滤波解，并对两个子滤波器进行重置包括：将各子滤波器中当前时刻的局部状态估计值
Figure FDA00025202048000000212
和
Figure FDA00025202048000000213
局部状态估计协方差阵P1(k)和P2(k)输入到主滤波器中按照预设公式进行融合，获取当前时刻的全局滤波解；
利用当前时刻的全局滤波解对两个子滤波器当前时刻的局部状态估计值和局部估计协方差阵进行重置。
6.根据权利要求1所述的基于联邦平滑的MPOS离线组合估计方法，其特征在于，所述用全局滤波解中的全局状态估计值校正当前时刻的MIMU的捷联解算结果直到联邦滤波结束包括：
用当前时刻的全局滤波解中的全局状态估计值去校正当前时刻的MIMU的位置、速度和姿态。主要包括速度修正、位置修正和姿态修正。
7.根据权利要求1所述的基于联邦平滑的MPOS离线组合估计方法，其特征在于，所述对两个子滤波器分别进行后向平滑解算包括：
对第一子滤波器以及第二子滤波器进行平滑初始化；
其中，
Figure FDA0002520204800000031
P1 S(N)＝P1(N)，上标S用于标记后向平滑值，N为MPOS进行组合估计的时间段的最后时刻，
Figure FDA0002520204800000032
为N时刻第一子滤波器的平滑估计值，P1 S(N)为N时刻第一子滤波器的平滑估计协方差阵；
Figure FDA0002520204800000033
为N时刻第一子滤波器的局部状态估计值，P1(N)为N时刻第一子滤波器的局部估计协方差阵；
对于k＝N-1,…,2,1,利用联邦滤波存储的信息F1(k)、
Figure FDA0002520204800000034
P1(k)、
Figure FDA0002520204800000035
和P1 -(k)进行R-T-S固定区间平滑后向平滑递推解算，得到第一子滤波器所有时刻的平滑估计值
Figure FDA0002520204800000036
和平滑估计协方差阵P1 S(k)；
递推解算后得到第二子滤波器N时刻的平滑估计值
Figure FDA0002520204800000037
平滑估计协方差阵
Figure FDA0002520204800000038
以及其它时刻的平滑估计值
Figure FDA0002520204800000039
平滑估计协方差阵
Figure FDA00025202048000000310
k＝N-1,…,2,1。
8.根据权利要求1所述的基于联邦平滑的MPOS离线组合估计方法，其特征在于，所述通过所有时刻的全局平滑估计结果计算出所有时刻的更高精度的MIMU的位置、速度和姿态包括：
将两个子滤波器的平滑估计值
Figure FDA0002520204800000041
和
Figure FDA0002520204800000042
平滑估计协方差阵P1 S(k)和
Figure FDA0002520204800000043
按照预设公式进行全局融合计算，获取所有时刻的全局平滑估计结果：
利用全局平滑估计值
Figure FDA0002520204800000044
计算出所有时刻的更高精度的MIMU的位置、速度和姿态。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现所述权利要求1-8中任一项所述方法的步骤。
10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现所述权利要求1-8中任一项所述方法的步骤。
11.基于联邦平滑的MPOS离线组合估计装置，其特征在于，所述装置包括：
设计模块，用于进行联邦滤波器结构设计，所述结构设计包括对两个子滤波器和一个主滤波器的设计；
建立模块，用于建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；
存储模块，用于存储当前时刻两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵、MIMU的捷联解算结果；
估计与重置模块，用于由主滤波器估计出全局滤波解，并对两个子滤波器进行重置；
校正模块，用于用全局滤波解中的全局状态估计值校正当前时刻的MIMU的捷联解算结果直到联邦滤波结束；
解算模块，用于通过预先存储的两个子滤波器的状态转移矩阵、局部状态估计值、局部状态估计协方差阵、局部状态一步预测、局部状态一步预测协方差阵，对两个子滤波器分别进行后向平滑解算，得到两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵；
全局融合计算模块，用于通过获取的两个子滤波器平滑后的所有时刻的状态估计值和状态估计协方差阵进行全局融合计算，得到所有时刻的全局平滑估计结果；
估计计算模块，用于通过所有时刻的全局平滑估计结果计算出所有时刻的更高精度的MIMU的位置、速度和姿态。
CN202010489028.1A 2020-06-02 2020-06-02 基于联邦平滑的mpos离线组合估计方法和装置 Active CN111811512B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010489028.1A CN111811512B (zh) 2020-06-02 2020-06-02 基于联邦平滑的mpos离线组合估计方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010489028.1A CN111811512B (zh) 2020-06-02 2020-06-02 基于联邦平滑的mpos离线组合估计方法和装置

Publications (2)

Publication Number Publication Date
CN111811512A true CN111811512A (zh) 2020-10-23
CN111811512B CN111811512B (zh) 2023-08-01

Family

ID=72848587

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010489028.1A Active CN111811512B (zh) 2020-06-02 2020-06-02 基于联邦平滑的mpos离线组合估计方法和装置

Country Status (1)

Country Link
CN (1) CN111811512B (zh)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
CN101178312A (zh) * 2007-12-12 2008-05-14 南京航空航天大学 基于多信息融合的航天器组合导航方法
CN101216319A (zh) * 2008-01-11 2008-07-09 南京航空航天大学 基于联邦ukf算法的低轨卫星多传感器容错自主导航方法
US20130085628A1 (en) * 2011-10-03 2013-04-04 Rongsheng Li Modified Kalman Filter for Generation of Attitude Error Corrections
US20160140729A1 (en) * 2014-11-04 2016-05-19 The Regents Of The University Of California Visual-inertial sensor fusion for navigation, localization, mapping, and 3d reconstruction
CN108896044A (zh) * 2018-07-23 2018-11-27 湖南格纳微信息科技有限公司 一种基于惯性导航和卫星导航的定位方法及装置
CN109737959A (zh) * 2019-03-20 2019-05-10 哈尔滨工程大学 一种基于联邦滤波的极区多源信息融合导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110823217A (zh) * 2019-11-21 2020-02-21 山东大学 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN111189441A (zh) * 2020-01-10 2020-05-22 山东大学 一种多源自适应容错联邦滤波组合导航系统及导航方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
CN101178312A (zh) * 2007-12-12 2008-05-14 南京航空航天大学 基于多信息融合的航天器组合导航方法
CN101216319A (zh) * 2008-01-11 2008-07-09 南京航空航天大学 基于联邦ukf算法的低轨卫星多传感器容错自主导航方法
US20130085628A1 (en) * 2011-10-03 2013-04-04 Rongsheng Li Modified Kalman Filter for Generation of Attitude Error Corrections
US20160140729A1 (en) * 2014-11-04 2016-05-19 The Regents Of The University Of California Visual-inertial sensor fusion for navigation, localization, mapping, and 3d reconstruction
CN108896044A (zh) * 2018-07-23 2018-11-27 湖南格纳微信息科技有限公司 一种基于惯性导航和卫星导航的定位方法及装置
CN109737959A (zh) * 2019-03-20 2019-05-10 哈尔滨工程大学 一种基于联邦滤波的极区多源信息融合导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110823217A (zh) * 2019-11-21 2020-02-21 山东大学 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN111189441A (zh) * 2020-01-10 2020-05-22 山东大学 一种多源自适应容错联邦滤波组合导航系统及导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
张樨等: "基于MIMU/GPS/EC的联邦卡尔曼滤波器设计", 《测试技术学报》 *
王勇军等: "联邦滤波算法与Gauss-Markov估计的统一性分析", 《火力与指挥控制》 *
陈晶等: "基于改进联邦Kalman滤波的组合校准方法研究", 《导航定位与授时》 *

Also Published As

Publication number Publication date
CN111811512B (zh) 2023-08-01

Similar Documents

Publication Publication Date Title
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN109470239B (zh) 磁场补偿方法，相关装置及计算机程序
CN110926468A (zh) 基于传递对准的动中通天线多平台航姿确定方法
CN108627152B (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN108375383B (zh) 多相机辅助的机载分布式pos柔性基线测量方法和装置
CN110346824B (zh) 一种车辆导航方法、系统、装置及可读存储介质
Rad et al. Optimal attitude and position determination by integration of INS, star tracker, and horizon sensor
Xue et al. In-motion alignment algorithm for vehicle carried SINS based on odometer aiding
CN113291493B (zh) 一种卫星多敏感器融合姿态确定方法和系统
Cui et al. In-motion alignment for low-cost SINS/GPS under random misalignment angles
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN115435817B (zh) Mems惯导安装误差的标定方法、存储介质和控制计算机
CN114002725A (zh) 一种车道线辅助定位方法、装置、电子设备及存储介质
JP2019120587A (ja) 測位システム及び測位方法
CN114413934A (zh) 一种车辆定位系统校正方法和装置
CN116007620A (zh) 一种组合导航滤波方法、系统、电子设备及存储介质
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN107764268B (zh) 一种机载分布式pos传递对准的方法和装置
CN111912427A (zh) 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN110375740B (zh) 车辆导航方法、装置、设备和存储介质
CN110160530B (zh) 一种基于四元数的航天器姿态滤波方法
CN107702718B (zh) 一种基于瞬间可观测度模型的机载pos机动优化方法与装置
CN111811512B (zh) 基于联邦平滑的mpos离线组合估计方法和装置
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN114111768B (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
GR01 Patent grant
GR01 Patent grant