[发明专利]GPS延迟时间自适应的扩展卡尔曼滤波导航方法

权利要求书

1.一种GPS延迟时间自适应的扩展卡尔曼滤波导航方法，其特征在于：包括

步骤1，准备数据；

1.1、读取传感器IMU采集的加速度和GPS采集的速度；

1.2、对原始数据做高阶低通滤波处理，以便消除数据跳动；

1.3、将处理后的数据送入先入先出队列FIFO进行缓存；

1.4、以一定频率，取出一段时间内的缓存数据，以最小化代价函数为目标，进行迭代优化直至找出最佳延迟时间t_d，建立关于GPS延迟时间的代价函数:

其中：

a_S(t)为t时刻对应的加速度计的采样值；

a_G(t-t_d)为比t时刻落后t_d时间对应的GPS差分加速度值；

M为缓存深度常量；

T_r为缓存时间；

以代价函数取得最小值为优化目标,以GPS的采集周期为步长，对t_d从0到M进行扫描，获取优化后t_d的值；

步骤2，执行自适应扩展卡尔曼滤波迭代；

2.1、取出步骤1中FIFO中的最新数据，以及延迟时间t_d；

2.2、执行先验估计过程：

其中：

Δt为算法迭代周期；

为载体系加速度计0偏向量；

为大地系的加速度向量；

为加速度计在载体系的测量值向量；

g为当地重力加速度标量；

为大地系东北天三轴速度向量；

为大地系东北天三轴位置向量；

D_BG为从载体系到大地系的方向余弦矩阵，其值可由缓存的姿态四元数q获得；

按照先验估计过程的非线性运动状态方程进行状态一步预测，这一过程可描述为：

定义状态向量

其中，

a_Bbx为载体系X轴方向上的加速度0偏；

a_Bby为载体系Y轴方向上的加速度0偏；

a_Bbz为载体系Z轴方向上的加速度0偏；

2.3、建立卡尔曼先验估计误差的协方差矩阵递推模型:

P(t+Δt)＝F·P(t)·F^T+Q(t)，

Q为过程误差的协方差矩阵，F为状态转移矩阵，

其中E为3×3的单位矩阵，0为3×3的零矩阵；

2.4、定义观测向量Z＝[v_x v_y v_z p_x p_y p_z]^T；

建立卡尔曼观测矩阵H：

其中三轴速度观测值v_x、v_y、v_z，三轴位置观测值p_x、p_y、p_z可由GPS接收机获得；

2.5、根据步骤一给出的延迟时间t_d，自适应计算速度、位置的测量误差协方差：

r(t_d)＝r₀·(1+k·arctan(t_d))

其中，r₀为速度或位置误差协方差的基准值，k代表协方差随时间延迟而做出非线性自适应增大的增益系数；

2.6、根据自适应的测量值误差协方差，更新卡尔曼增益：

K(t+Δt)＝P(t+Δt)H^T(t+Δt)(H(t+Δt)P(t+Δt)H(t+Δt)+R(t+Δt,t_d))

其中：

P(t+Δt)为t+Δt时刻的先验误差协方差矩阵；

H(t+Δt)为t+Δt时刻的卡尔曼观测矩阵；

上式中测量误差的协方差定义为：R＝R(t+Δt,t_d)；

2.7、更新卡尔曼后验估计：

其中：

K(t+Δt)为t+Δt时刻的卡尔曼增益矩阵；

Z(t+Δt)为t+Δt时刻的测量值向量；

2.8、更新后验估计的误差协方差矩阵：

P⁺(t+Δt)＝(E-K(t+Δt)H(t+Δt))P^-(t+Δt)；

步骤3，执行无监督惯导递推，同时返回步骤1，去准备新一轮的数据。