基于误差状态相关性的ST-EKF均方误差阵初始化方法和系统技术方案

本发明专利技术公开基于误差状态相关性的ST

【技术实现步骤摘要】
基于误差状态相关性的ST
‑
EKF均方误差阵初始化方法和系统


[0001]本专利技术属于组合导航控制领域，更具体地，涉及基于误差状态相关性的ST
‑
EKF均方误差阵初始化方法和系统。

技术介绍

[0002]为解决传统组合导航系统误差状态模型的线性化误差问题，近年来ST
‑
EKF(State Transformed Extended Kalman Filtering，状态变换扩展卡尔曼滤波)受到了导航领域学者的关注。ST
‑
EKF考虑了姿态误差(即失准角)导致的计算导航坐标系和真实导航坐标系之间的偏差，重新定义了非线性的速度误差状态，在大失准角的情况下，相比传统的滤波算法具有更好的估计性能。然而与变换后的误差状态对应的均方误差阵的初始化问题，却没有引起足够的重视。
[0003]均方误差阵定义为状态估计与真值间偏差的数学期望，矩阵对角线元素是各状态分量估计的方差，其平方根表征了状态估计的不确定度。均方误差阵的非对角线元素为协方差，反应了不同状态分量估计误差之间的相关性。传统的惯性基组合系统采用的是基于线性误差状态的标准KF算法，可认为初始预估的各状态误差之间不相关或弱相关，因此均方误差阵通常可初始化为对角矩阵。该方法也一直应用于ST
‑
EKF均方误差阵的初始化。具体做法是：先将传统误差状态转换为ST
‑
EKF定义的新误差状态，然后计算各新状态分量初始估计的方差，最后将均方误差阵设置为对角矩阵。
[0004]但是，ST
‑
EKF重新定义了非线性的误差状态，各状态分量间表现出了较强的相关性，以对角矩阵粗略的对均方误差阵进行初始化并不合理，在大失准角、高航速的情况下，误差状态的估计性能将会受到一定的影响。

技术实现思路

[0005]针对现有技术的缺陷，本专利技术的目的在于提供基于误差状态相关性的ST
‑
EKF均方误差阵初始化方法和系统，旨在解决现有技术将均方误差阵初始化为对角矩阵，导致误差状态估计性能受影响的问题。
[0006]为实现上述目的，第一方面，本专利技术提供了一种基于误差状态相关性的ST
‑
EKF均方误差阵初始化方法，包括：
[0007]S1.获取KF组合导航系统的初始误差状态，所述误差状态包括姿态误差、速度误差和位置误差；
[0008]S2.以初始误差状态计算KF组合导航系统的均方误差阵；
[0009]S3.根据ST
‑
EKF定义的非线性误差状态和KF误差状态间的变换关系，确定误差状态转换矩阵；
[0010]S4.根据KF均方误差阵和误差状态转换矩阵，得到ST
‑
EKF状态变换后组合导航系统的均方误差阵，完成初始化。
[0011]优选地，所述KF组合导航系统的均方误差阵P
n
的计算公式如下：
[0012][0013]其中，P
φ
、P
δv
和P
δp
分别为与φ、δv和δp对应的均方误差阵，且都为对角矩阵，φ、δv和δp分别为姿态误差、速度误差和位置误差，03×3为3
×
3零矩阵。
[0014]优选地，所述根据ST
‑
EKF定义的非线性误差状态表示为EKF定义的非线性误差状态表示为δv
φ
为状态变换后的非线性速度误差δv
φ
表示为：
[0015][0016]其中，δv为姿态误差，为估计速度，(
·
×
)为反对称矩阵运算符，φ为姿态误差。
[0017]优选地，所述误差状态转换矩阵A
sn
的计算公式如下：
[0018][0019]其中，I3×3为3
×
3单位矩阵，03×3为3
×
3零矩阵，为估计速度的反对称矩阵。
[0020]优选地，所述ST
‑
EKF状态变换后组合导航系统的均方误差阵P
s
的计算公式如下：
[0021]P
s
＝A
sn
P
n
A
snT
[0022]其中，A
sn
为误差状态转换矩阵，P
n
为组合导航系统的均方误差阵。
[0023]为实现上述目的，第二方面，本专利技术提供了一种基于误差状态相关性的ST
‑
EKF均方误差阵初始化系统，包括：处理器和存储器；所述存储器，用于存储计算机执行指令；所述处理器，用于执行所述计算机执行指令，使得第一方面所述的方法被执行。
[0024]为实现上述目的，第三方面，本专利技术提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，当所述计算机程序在处理器上运行时，使得所述处理器执行第一方面所述的方法。
[0025]总体而言，通过本专利技术所构思的以上技术方案与现有技术相比，具有以下有益效果：
[0026]本专利技术提供基于误差状态相关性的ST
‑
EKF均方误差阵初始化方法和系统，首先以传统的误差状态进行均方误差阵计算，然后根据ST
‑
EKF重新定义的非线性误差状态和传统误差状态间的变换关系，分析新误差状态分量间的强相关性，确定误差状态转换矩阵；最后将新的误差状态代入均方误差阵计算公式，获得与之对应的均方误差阵转换模型，完成初始化。该方式具有良好的通用性，各种失准角均适用，尤其是面对大失准角、高航速的情况。
附图说明
[0027]图1是本专利技术提供的一种基于误差状态相关性的ST
‑
EKF均方误差阵初始化方法的流程图。
具体实施方式
[0028]为了使本专利技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对
本专利技术进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本专利技术，并不用于限定本专利技术。
[0029]如图1所示，本专利技术提供了一种基于误差状态相关性的ST
‑
EKF均方误差阵初始化方法，包括：
[0030]S1.获取KF组合导航系统的初始误差状态，所述误差状态包括姿态误差、速度误差和位置误差。
[0031]S2.以初始误差状态计算KF组合导航系统的均方误差阵。
[0032]S3.根据ST
‑
EKF定义的非线性误差状态和KF误差状态间的变换关系，确定误差状态转换矩阵。
[0033]S4.根据KF均方误差阵和误差状态转换矩阵，得到ST
‑
EKF状态变换后组合导航系统的均方误差阵，完成初始化。
[0034]一、KF均方误差阵的设置
[0035]设传统组合导航系统KF误差状态矢量为X
n
＝[φ
T δv
T δp
T
]T
...

【技术保护点】

【技术特征摘要】
1.一种基于误差状态相关性的ST
‑
EKF均方误差阵初始化方法，其特征在于，包括：S1.获取KF组合导航系统的初始误差状态，所述误差状态包括姿态误差、速度误差和位置误差；S2.以初始误差状态计算KF组合导航系统的均方误差阵；S3.根据ST
‑
EKF定义的非线性误差状态和KF误差状态间的变换关系，确定误差状态转换矩阵；S4.根据KF均方误差阵和误差状态转换矩阵，得到ST
‑
EKF状态变换后组合导航系统的均方误差阵，完成初始化。2.如权利要求1所述的方法，其特征在于，所述KF组合导航系统的均方误差阵P
n
的计算公式如下：其中，P
φ
、P
δv
和P
δp
分别为与φ、δv和δp对应的均方误差阵，且都为对角矩阵，φ、δv和δp分别为姿态误差、速度误差和位置误差，03×3为3
×
3零矩阵。3.如权利要求1所述的方法，其特征在于，所述根据ST
‑
EKF定义的非线性误差状态表示为δv
φ
为状态变换后的非线性速度误差δv
φ
表示为：其中，δv为姿态误差，为估计速度，(
...

【专利技术属性】
技术研发人员：唐君，卞鸿巍，马恒，王荣颖，文者，胡耀金，丁贤，
申请(专利权)人：中国人民解放军海军工程大学，
类型：发明
国别省市：