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

文档序号:19457047发布日期:2019-12-20 20:05阅读:来源:国知局

技术特征:

1.一种基于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-1ht(hpk|k-1ht+r)-1;其中,h为设定的观测矩阵,r为设定的噪声方差矩阵;其中,观测矩阵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),通过卡尔曼滤波计算得到初始位置误差、初始速度误差和初始姿态失准角。

当前第2页1 2 3 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1