[发明专利]一种提高基于MEMS陀螺的无人艇协同导航定位精度的方法

权利要求书

1.一种提高基于MEMS陀螺的无人艇协同导航定位精度的方法，其特征在于:

（1）建立正向滤波模型

从艇上的水声通信模块接收主艇发来的信息，通过水声信号发送和接收的时间差乘以声速计算主从艇的距离作为观测量，利用多普勒测速仪测量的速度和MEMS陀螺测出的航向，通过扩展卡尔曼滤波估计从艇的位置和MEMS陀螺的误差

状态一步预测方程为：

式中，x_k、y_k表示从艇在k时刻的位置，v_k为从艇速度，t表示推位时间间隔，表示MEMS陀螺测得的航向，表示MEMS陀螺测量航向的误差，在方程中用其估计值修正了航向测量值的偏差：

X_k+1＝f(X_k,u_k,t)+w_k

线性化后得，

X_k+1＝F_kX_k+B_ku_k+w_k

式中，系统噪声w_k～N(0,Q_k)，量测方程为

式中，观测量Z_k表示主从艇的距离r，x_a、y_a表示主艇的位置，x_b、y_b表示从艇位置，V_k～N(0,R_k)为量测噪声；

（2）存储正向滤波数据

在滤波的同时存储所测量的从艇速度、航向和主从艇的距离；

（3）逆向滤波系统模型建立

逆向滤波相比正向滤波速度等值反向，即令建立状态一步预测模型：

X_k-1＝f(X_k,u_k,t)+w_k

线性化后得，

X_k-1＝F_kX_k+B_ku_k+w_k

式中，

系统噪声w_k～N(0,Q_k)；

（4）建立逆向滤波量测模型

式中，观测量Z_k表示主从艇的距离r，x_a、y_a表示主艇的位置，x_b、y_b表示从艇位置，V_k-N(0,R_k)为测量噪声；

（5）利用建立的系统模型和量测模型，反向利用正向滤波时存储的从艇速度、航向进行时间更新，利用主从艇的距离进行量测更新，用扩展卡尔曼滤波估计从艇的位置和MEMS陀螺的误差，

时间更新：

量测更新：

P_k＝(I-K_kH_k)P_k/k-1

式中，为滤波输出的状态估计，P_k/k-1为状态和及其方差一步预测，K_k为滤波增益；

（6）正向滤波

正向滤波回到断点。