本发明涉及惯性导航系统领域，具体涉及一种惯性导航初始基准的估计与补偿方法。
背景技术：
在正式开始导航工作之前，惯性导航系统需要知道一些初始条件，例如初始姿态、初始速度和初始位置；通常情况初始速度和初始位置易于获得，而初始姿态中水平两个方向的初始姿态角通过自对准比较容易获得，但是航向角通过自对准获得则需要较长的时间或者需借助外部瞄准等辅助系统；若是为实现快速性的要求的情况下，航向角的初值误差会比较大，这样就会导致惯性导航的误差快速积累，导致导航的速度和位置精准度过低；因此需要利用惯性/卫星组合导航系统对姿态初值进行修正，以提高姿态角解算精度，达到提高中制导与末制导交班精度的目的。
技术实现要素：
本发明的目的是提供一种惯性导航初始基准的估计与补偿方法，适用于惯性/卫星组合导航系统中对姿态角误差有约束条件的飞行器导航制导方案选取，解决在惯性导航初始对准误差较大的情况下，对姿态角进行修正的问题。
为了解决上述技术问题，本发明的实施例提供了一种惯性导航初始基准的估计与补偿方法，惯性导航系统接收卫星导航提供的位置、速度信息，以及惯性导航系统计算得到的位置、速度信息，该方法包括如下步骤：
步骤a)建立初始姿态误差估计状态方程：
步骤b)建立姿态初值误差估计观测方程：
Z＝HX+V；
步骤c)计算观测量，将惯性导航速度和位置与卫星导航给出的速度和位置作差作为观测量：
步骤d)将状态方程和观测方程离散化：
Xk＝Φk,k-1Xk-1+Wk-1
Zk＝HkXk+Vk；
步骤e)使用卡尔曼滤波方程计算：
Pk＝(I-KkHk)Pk|k-1；
步骤f)姿态误差角的修正
姿态角偏差：
修正后的姿态角：
程序角计算：
ψcx＝ψcx-δψ·k
γcx＝γcx-δγ·k；
步骤g)速度和位置误差的修正
导航速度修正：V＝V-W·Φ0，
导航位置修正：X＝X-S·Φ0。
其中，所述步骤a中：
X＝[φx0 φy0 φz0 δVx0 δVy0 δVz0 δX0 δY 0δZ0]T
F＝O9×9；
初始姿态误差角：φx0、φy0、φz0，
速度初始误差：δVx0、δVy0、δVz0，
位置初始误差：δX0、δY0、δZ0。
其中，所述步骤b中：
其中，所述I3×3为3行3列的单位矩阵，Wx、Wy、Wz为视速度，Sx、Sy、Sz为视位置。
其中，所述步骤c中，VxINS，VyINS，VzINS为惯性导航速度，XINS，YINS，ZINS为惯性导航位置，VxGNSS，VxGNSS，VxGNSS为卫星导航给出的速度，XGNSS，YGNSS，YGNSS为卫星导航给出的位置。
其中，所述步骤d中，Φk,k-1＝I9×9，Qk-1＝O9×9，Hk＝H。
其中，所述步骤e中，滤波方程初值为：X0＝O9×1，P0和R0根据具体情况选取。
其中，所述步骤f中，ψ0、γ0为当前俯仰角、当前偏航角，当前滚动角，δψ，δγ为俯仰姿态角偏差、偏航姿态角偏差、滚动姿态角偏差；ψcx、γcx为俯仰程序角、偏航程序角、滚动程序角。
其中，所述步骤f中，系数k计算公式如下：
其中，t为当前时间，t0为修正起始时刻，Δt为修正时间。
其中，所述步骤g中，
本发明通过卫星导航提供的位置、速度信息，和惯性导航计算得到的位置、速度信息采用卡尔曼滤波的方式，估计惯性导航初始基准误差，修正时结合姿态控制要求采用程序角调姿的方式，对姿态角进行修正，同时对速度和位置进行修正，最终实现提高姿态角解算精度，达到提高中制导与末制导交班精度的目的。
与现有技术相比，本发明的有益之处在于：1)满足惯性导航快速初始对准的要求，缩短运载火箭和导弹武器的发射准备时间；2)在飞行过程中对初始姿态误差进行估计和补偿，以消除初始姿态误差；3)还可以消除在飞行过程中对初始姿态误差带来的速度和位置误差。
附图说明
图1为本发明实施例的惯性导航初始基准的估计与补偿方法流程图。
具体实施方式
为使本发明的目的、技术方案及优点更加清楚明白，以下参照附图并举实施例，对本发明进一步详细说明。
本发明的原理如下。本发明根据卫星导航提供的位置、速度信息，和惯性导航计算得到的位置、速度信息采用卡尔曼滤波的方式，估计惯性导航初始基准误差，修正时结合姿态控制要求采用程序角调姿的方式，对姿态角进行修正，同时对速度和位置进行修正。
下面结合说明书附图对本发明作进一步详细描述。图1示出了本发明实施例的惯性导航初始基准的估计与补偿方法流程图。参见图1，根据本发明实施例的惯性导航初始基准的估计与补偿方法包括以下步骤：
步骤a)建立初始姿态误差估计状态方程：
步骤b)建立姿态初值误差估计观测方程：
Z＝HX+V；
步骤c)计算观测量，将惯性导航速度和位置与卫星导航给出的速度和位置作差作为观测量：
步骤d)将状态方程和观测方程离散化：
Xk＝Φk,k-1Xk-1+Wk-1
Zk＝HkXk+Vk；
步骤e)使用卡尔曼滤波方程计算：
Pk＝(I-KkHk)Pk|k-1；
步骤f)姿态误差角的修正，其中
姿态角偏差：
修正后的姿态角：
程序角计算：
ψcx＝ψcx-δψ·k
γcx＝γcx-δγ·k；
步骤g)速度和位置误差的修正，其中
导航速度修正：V＝V-W·Φ0，
导航位置修正：X＝X-S·Φ0。
其中，所述步骤a中：
X＝[φx0 φy0 φz0 δVx0 δVy0 δVz0 δX0 δY0 δZ0]T
F＝O9×9；
初始姿态误差角：φx0、φy0、φz0，
速度初始误差：δVx0、δVy0、δVz0，
位置初始误差：δX0、δY0、δZ0。
其中，所述步骤b中：
其中，所述I3×3为3行3列的单位矩阵，Wx、Wy、Wz为视速度，Sx、Sy、Sz为视位置。
其中，所述步骤c中，VxINS，VyINS，VzINS为惯性导航速度，XINS，YINS，ZINS为惯性导航位置，VxGNSS，VxGNSS，VxGNSS为卫星导航给出的速度，XGNSS，YGNSS，YGNSS为卫星导航给出的位置。
其中，所述步骤d中，Φk,k-1＝I9×9，Qk-1＝O9×9，Hk＝H。
其中，所述步骤e中，滤波方程初值为：X0＝O9×1，P0和R0根据具体情况选取。
其中，所述步骤f中，ψ0、γ0为当前俯仰角、当前偏航角，当前滚动角，δψ，δγ为俯仰姿态角偏差、偏航姿态角偏差、滚动姿态角偏差；ψcx、γcx为俯仰程序角、偏航程序角、滚动程序角。
其中，所述步骤f中，系数k计算公式如下：
其中，t为当前时间，t0为修正起始时刻，Δt为修正时间。
其中，所述步骤g中，
本发明可以满足惯性导航快速初始对准的要求，在飞行过程中对初始姿态误差进行估计和补偿，以消除初始姿态误差，以及由此带来的速度和位置误差，对缩短导弹武器发射准备时间具有重要意义。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。