[发明专利]基于改进的非线性鲁棒滤波算法的系统状态估计方法

摘要

一种空间导航技术领域的基于改进的非线性鲁棒滤波算法的系统状态估计方法，该方法基于Huber的非线性回归算法，使得在测量输出方程为非线性、测量噪声为非高斯分布的情况下，得到较高的滤波精度和较稳定的滤波输出。

主权项

1.一种基于改进的非线性鲁棒滤波方法的系统状态估计方法，其特征在于，包括以下步骤：第一步、k＝1，初始化系统状态变量和系统状态误差方差阵第二步、构造非线性回归模型：ykx‾k=g(xk)xk+vk-δk,]]>其中：是扩展卡尔曼滤波的一步状态预测，可由扩展卡尔曼滤波一步预测方程递推得到，δ_k是真实状态与预测状态之间的误差；设ζ=vk-δk]]>且满足：E(ζ·ζT)=Rk00P‾k=SkT·Sk,]]>其中：R_k是主高斯测量噪声方差阵，是状态预测误差方差阵；第三步、将非线性回归模型的两边同时乘以非线性回归问题可以转换为z_k＝h(x_k)+ξ_k，其中：h(xk)=Sk-1g(xk)xkT,]]>ξk=Sk-1ζ]]>和zk=Sk-1ykx‾kT;]]>定义代价函数：其中：e_i是残差向量e＝z_k-h(x_k)的第i个元素，m是向量e的维数；Huber评价函数为ρ(ei)=12ei2|ei|<γγ|ei|-12γ2|ei|≥γ,]]>对代价函数求导后可得其中：γ为调节参数；定义Ψ＝diag[ψ(e_i)]，则对代价函数求导后得到：其中：M_k是h(x_k)的雅克比矩阵，展开后用最小二乘方法求解，其初值设为xk(0)=(MkTMk)-1MkTzk,]]>其迭代收敛解为xk(j+1)=(MkTΨ(j)Mk)-1MkTΨ(j)zk,]]>其中：j是迭代次数，在求取迭代收敛解时，需由具体的应用实例选取对应的迭代收敛阈值；第四步、将通过迭代状态估计的最终矩阵Ψ根据状态预测和测量预测残差分解为Ψ=Ψy00Ψx,]]>则鲁棒卡尔曼滤波增益矩阵为：Kk=P‾k1/2Ψx-1P‾kT/2Hk(HkP‾k1/2Ψx-1P‾kT/2HkT+Rk1/2Ψy-1RkT/2)-1,]]>其中：H_k是测量输出方程雅克比矩阵，测量更新后的系统状态，即由向量所指代、通过标准卡尔曼滤波递推方程得到的系统状态，x^k=x‾k+Kk(yk-y‾k);]]>状态误差方差阵测量更新矩阵为：P^k=(I-KkHk)P‾k1/2Ψ-1P‾kT/2;]]>第五步、k＝k+1，并返回到第一步进行下一时刻的状态估计。