一种基于GPS的飞行中惯性初始基准偏差估计方法与流程

本发明涉及制导与导航技术领域，特别涉及一种基于gps的飞行中惯性初始基准偏差估计方法。

背景技术：

某些导弹采用间接瞄准或传递对准，会产生较大的初始基准偏差，这样会致使惯性导航误差快速累积，因此有必要采取措施估计初始基准精度以抑制惯性导航误差发散速度。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供了一种基于gps的飞行中惯性初始基准偏差估计方法，该方法根据惯性导航误差传播机理建立惯性初始基准偏差模型，利用gps信息作为观测量，采用标准卡尔曼滤波方法估计惯性初始基准偏差。

本发明的上述目的通过以下方案实现：

一种基于gps的飞行中惯性初始基准偏差估计方法，包括以下步骤：

(1)、在时刻k，计算状态向量预测值为时刻k-1的状态向量估计值，所述状态向量估计值的初始值设定为零向量；其中，在卡尔曼滤波中，定义状态向量x＝[φx0φy0φz0δx0δy0δz0δvx0δvy0δvz0]^t；φx0、φy0、φz0分别为导弹在惯性坐标系下的x向初始姿态失准角、y向初始姿态失准角、z向初始姿态失准角；δx0、δy0、δz0分别为导弹在惯性坐标系下的x向初始位置误差、y向初始位置误差、z向初始位置误差；δvx0、δvy0、δvz0分别为导弹在惯性坐标系下的x向初始速度误差、y向初始速度误差、z向初始速度误差；

(2)、在时刻k，预测协方差矩阵pk|k-1＝pk-1；pk-1为时刻k-1的估计协方差矩阵，所述估计协方差矩阵的初始值为p0，p0为设定的9×9维矩阵；

(3)、在时刻k，计算增益矩阵kk＝pk|k-1h^t(hpk|k-1h^t+r)^-1；其中，h为设定的观测矩阵，r为设定的噪声方差矩阵：

(4)、计算时刻k的状态向量估计值其中，zk为外部提供的时刻k的观测向量；在卡尔曼滤波中，定义观测向量z＝[δxδyδzδvxδvyδvz]^t；δx、δy、δz分别为惯导系统与gps系统测量的x向位置之差、y向位置之差、z向位置之差；δvx、δvy、δvz分别为惯导系统与gps系统测量的x向速度之差、y向速度之差、z向速度之差；

(5)、计算时刻k估计协方差矩阵pk＝(i-kkh)pk|k-1，i为单位矩阵；

(6)、在设定的初值基准偏差估计周期内，重复步骤(1)～(5)，通过卡尔曼滤波计算得到初始位置误差、初始速度误差和初始姿态失准角。

上述的基于gps的飞行中惯性初始基准偏差估计方法，在步骤(3)中，观测矩阵h和噪声方差矩阵r分别设定为：

其中：sx、sy、sz分别为惯导系统提供的惯性系x向、y向、z向视位置；wx、wy、wz分别为惯导系统提供的惯性系x向、y向、z向视速度；分别为设定惯性系下x向、y向、z向位置观测噪声方差；分别为设定的惯性系下x向、y向、z向速度观测噪声方差。

本发明与现有技术相比，具有以下优点：

经数学仿真技术和搭载飞行试验验证，基于gps的飞行中惯性初始基准偏差估计方法能够有效地估计出惯性基准偏差，达到提高导航系统的精度和可靠性的目的。

附图说明

图1为本发明的基于gps的飞行中惯性初始基准偏差估计方法的流程图。

具体实施方式

下面结合附图和具体实例对本发明作进一步详细的描述：

本发明根据惯性导航误差传播机理建立惯性初始基准偏差模型，利用gps信息作为观测量，采用标准卡尔曼滤波方法估计惯性初始基准偏差，具体实现流程如图1所示。

首先，建立初始基准偏差估计状态方程：

其中：状态向量x＝[φx0φy0φz0δx0δy0δz0δvx0δvy0δvz0]^t；φx0、φy0、φz0分别为导弹在惯性坐标系下的x向初始姿态失准角、y向初始姿态失准角、z向初始姿态失准角；δx0、δy0、δz0分别为导弹在惯性坐标系下的x向初始位置误差、y向初始位置误差、z向初始位置误差；δvx0、δvy0、δvz0分别为导弹在惯性坐标系下的x向初始速度误差、y向初始速度误差、z向初始速度误差；系统矩阵f为9×9维零矩阵。

然后，建立惯性初始基准偏差估计的观测方程：

z＝hx+v；

其中：观测向量z＝[δxδyδzδvxδvyδvz]^t；δx、δy、δz分别为惯导系统与gps系统测量的x向位置之差、y向位置之差、z向位置之差；δvx、δvy、δvz分别为惯导系统与gps系统测量的x向速度之差、y向速度之差、z向速度之差；v为观测噪声向量；h为观测矩阵，在本发明中具体设定为：

其中：sx、sy、sz分别为惯导系统提供的惯性系x向、y向、z向视位置；wx、wy、wz分别为惯导系统提供的惯性系x向、y向、z向视速度。

基于以上的卡尔曼滤波方程，本发明的基于gps的飞行中惯性初始基准偏差估计方法的实现步骤如下：

(1)、在时刻k，计算状态向量预测值为时刻k-1的状态向量估计值，所述状态向量估计值的初始值设定为零向量；其中，在卡尔曼滤波中，定义状态向量x＝[φx0φy0φz0δx0δy0δz0δvx0δvy0δvz0]^t；φx0、φy0、φz0分别为导弹在惯性坐标系下的x向初始姿态失准角、y向初始姿态失准角、z向初始姿态失准角；δx0、δy0、δz0分别为导弹在惯性坐标系下的x向初始位置误差、y向初始位置误差、z向初始位置误差；δvx0、δvy0、δvz0分别为导弹在惯性坐标系下的x向初始速度误差、y向初始速度误差、z向初始速度误差；

(2)、在时刻k，预测协方差矩阵pk|k-1＝pk-1；pk-1为时刻k-1的估计协方差矩阵，所述估计协方差矩阵的初始值为p0，p0为设定的9×9维矩阵；

(3)、在时刻k，计算增益矩阵kk＝pk|k-1h^t(hpk|k-1h^t+r)^-1；其中，h为设定的观测矩阵，r为设定的噪声方差矩阵：

其中：sx、sy、sz分别为惯导系统提供的惯性系x向、y向、z向视位置；wx、wy、wz分别为惯导系统提供的惯性系x向、y向、z向视速度；分别为设定惯性系下x向、y向、z向位置观测噪声方差；分别为设定的惯性系下x向、y向、z向速度观测噪声方差；

(4)、计算时刻k的状态向量估计值其中，zk为外部提供的时刻k的观测向量；在卡尔曼滤波中，定义观测向量z＝[δxδyδzδvxδvyδvz]^t；δx、δy、δz分别为惯导系统与gps系统测量的x向位置之差、y向位置之差、z向位置之差；δvx、δvy、δvz分别为惯导系统与gps系统测量的x向速度之差、y向速度之差、z向速度之差；

(5)、计算时刻k估计协方差矩阵pk＝(i-kkh)pk|k-1，i为单位矩阵；

(6)、在设定的初值基准偏差估计周期内，重复步骤(1)～(5)，通过卡尔曼滤波计算得到初始位置误差、初始速度误差和初始姿态失准角。

在具体工程实现时，本发明的基于gps的飞行中惯性初始基准偏差估计方法由固化在弹上计算机中的软件实现，在导弹飞行过程中，飞行控制软件调用此方法，对导弹初始惯性基准偏差进行估计，提高导弹的导航精度及命中精度。

以上所述，仅为本发明一个具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。