[发明专利]一种具有鲁棒特性的协同导航滤波方法

权利要求书

1.一种具有鲁棒特性的协同导航滤波方法，其特征在于，包括以下步骤：

(1)在协同导航从艇定位中，利用投影统计对从艇接收到的来自主艇的量测信息进行异常判断，包括以下分步骤：

1)获得k个时刻的N维状态向量的中位向量，表示为：

x_k表示第k个时刻的N维状态向量，k＝1，…K，

x_nk代表x_k中第n个元素，

med代表取中位元素；

2)计算每个状态向量关于中值偏差的单位向量，其中，偏差向量u_k：

u_k＝x_k-m，k＝1....K

由此得到状态向量关于中值偏差的单位向量v_k为：

3)确定每个状态向量x_k在各单位向量v_k上的投影：

第k个时刻的状态向量的投影向量表示为z_k＝[z_1k，z_2k，...z_Kk]^T，

其中，H表示向量x转置，

4)对每个z_k，通过中值和完全中值偏差来计算标准投影p_nk，

其中，c为调节参数，

MAD_k表示第k个时刻的值的完全中值偏差，

5)对所有k值重复上述2)、3)、4)计算过程，则第K个点的PS值为

得到PS值后，根据该量测点的量测值相对于量测点簇的偏离程度，赋予该量测点的量测值的权值ω_i；

(2)获取量测信息的权值后，建立基于改进Huber估计的协同导航鲁棒滤波模型：

由协同导航的运动方程和量测方程得到非线性模型：

其中，下标k代表在时刻k时，x为状态向量，y为量测值，u为系统输入，v为系统噪声，w为量测噪声，v与w均服从零均值白噪声，量测噪声协方差阵为R，

量测方程近似写作：因此协同导航非线性模型改写为：

其中是协同导航从艇的位置预测值，表示实际值与预测值的差，

矩阵H_k定义为：

其中，是量测噪声的平均值；

定义以下式子：

其中，R_k是k时刻的量测噪声协方差阵；

P_k/k-1为一步预测误差均方差；

则非线性模型可另写为：

Z_k＝G_kx_k+ξ_k

定义残差表示估计值和量测值之间的差，ζ＝Gx-Z，

定义函数则对角矩阵其中ω_i为量测值的权值，由步骤5)计算得到，

将对角矩阵Ψ分为两部分：

Ψ_y与Ψ_x为重新构造观测模型后利用Huber估计求得的对角矩阵，将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新，实现滤波鲁棒性。

2.根据权利要求1所述的具有鲁棒特性的协同导航滤波方法，其特征在于，将权值对角矩阵Ψ代入卡尔曼滤波中进行滤波更新的过程如下：

时间更新

P_k/k-1＝Φ_k，k-1P_k-1Φ^T_k，k-1+Q_k

量测更新

其中Φ_k，k-1为状态过渡矩阵；

Q_k是k时刻的系统噪声协方差阵；

Ψ_y与Ψ_x为重新构造观测模型后利用Huber估计求得的对角矩阵。