[发明专利]一种自适应容积卡尔曼滤波方法

摘要

本发明涉及的是一种自适应容积卡尔曼滤波方法，特别是涉及一种带渐消记忆时变噪声统计估值器的自适应容积卡尔曼滤波方法。本发明包括下列步骤：（1）设定初始参数；（2）时间更新；（3）量测更新；（4）构造渐消记忆时变噪声统计估值器；（5）实时估计和修正噪声。相比于标准容积卡尔曼滤波方法，该方法不要求精确已知噪声的先验统计特性，具有应对噪声变化的自适应能力，且噪声统计估值器递推公式简单，更容易实现，且对噪声统计的估计是无偏的。

主权项

1.一种自适应容积卡尔曼滤波方法，包括如下步骤：（1）设定初始参数：设定初始时刻系统状态值x₀，初始时刻状态协方差P₀，初始时刻系统噪声均值q的渐消记忆时变噪声统计估值器初始时刻系统噪声协方差Q的渐消记忆时变噪声统计估值器初始时刻系统噪声均值r的渐消记忆时变噪声统计估值器初始时刻系统噪声协方差R的渐消记忆时变噪声统计估值器遗忘因子b；（2）时间更新：进行自适应容积卡尔曼滤波基于标准容积卡尔曼滤波的时间更新：Pk-1|k-1=Sk-1|k-1Sk-1|k-1T,]]>Xi,k-1|k-1=Sk-1|k-1ξi+x^k-1|k-1,]]>Xi,k-1|k-1*=f(Xi,k-1|k-1),]]>在k时刻的状态预测值中融入系统噪声均值q的渐消记忆时变噪声统计估值器x^k|k-1=1mΣi=1mf(Xi,k-1|k-1)+q^k-1,]]>在k时刻的误差协方差预测值P_k|k-1中融入系统噪声协方差差Q的渐消记忆时变噪声统计估值器Pk|k-1=1mΣi=1mXi,k|k-1*Xi,k|k-1*T-x^k|k-1x^k|k-1T+Q^k-1,]]>其中，P_k-1|k-1为k-1时刻的误差协方差；S_k-1|k-1为P_k-1|k-1乔里斯基因子；X_i,k-1|k-1为容积点集；m=2n，n为状态向量的维数；记n维单位列向量e=[1,0,…，0]^T，使用符号[1]表示对e的元素进行全排列和改变元素符号产生的点集，称为完整全对称点集，[1]_i表示点集[1]中的第i个点；为k-1时刻的状态估计值；为通过状态方程传播的容积点集；f(□)为非线性状态方程；为k时刻的状态预测值；P_k|k-1为k时刻的状态误差协方差预测值；为系统噪声均值q的渐消记忆时变噪声统计估值器；为系统噪声协方差Q的渐消记忆时变噪声统计估值器；（3）量测更新：进行自适应容积卡尔曼滤波基于标准容积卡尔曼滤波的量测更新：Pk|k-1=Sk|k-1Sk|k-1T,]]>Xi,k-1|k-1=Sk-1|k-1ξi+x^k-1|k-1,]]>z_i,k|k-1=h(X_i,k|k-1)，在k时刻的量测预测值中融入量测噪声均值r的渐消记忆时变噪声统计估值器z^k|k-1=1mΣi=1mh(Xi,k|k-1)+r^k,]]>在自相关协方差矩阵P_zz,k|k-1中融入量测噪声协方差R的渐消记忆时变噪声统计估值器Pzz,k|k-1=1mΣi=1mzi,k|k-1zi,k|k-1T-z^k|k-1z^k|k-1T+R^k,]]>结合标准容积卡尔曼滤波中互相关协方差矩阵P_xz,k|k-1，卡尔曼增益K_k，k时刻的状态估计值k时刻的状态误差协方差估计值P_k|k的完成量测更新：Pxz,k|k-1=1mΣi=1mXi,k|k-1zi,k|k-1T-x^k|k-1z^k|k-1T,]]>Kk=Pxz,k|k-1Pzz,k|k-1-1,]]>x^k|k=x^k|k-1+Kk(zk-z^k|k-1),]]>Pk|k=Pk|k-1-KkPzz,k|k-1KkT,]]>其中，S_k|k-1为P_k|k-1乔里斯基因子；X_i,k|k-1为容积点集；z_i,k|k-1为通过量测方程传播的容积点集；h(□)为非线性量测函数；为k时刻的量测预测值；为量测噪声均值r的渐消记忆时变噪声统计估值器；P_zz,k|k-1为自相关协方差矩阵；为量测噪声协方差R的渐消记忆时变噪声统计估值器；P_xz,k|k-1为互相关协方差矩阵；K_k为卡尔曼增益；为k时刻的状态估计值；z_k为k时刻的量测值；P_k|k为k时刻的状态误差协方差估计值；（4）构造渐消记忆时变噪声统计估值器：构造次优MAP常值噪声统计估值器；分析次优MAP常值噪声统计估值器的无偏性，构造次优无偏MAP常值噪声统计估值器；在次优无偏MAP常值噪声统计估值器的基础上，借助渐消记忆指数加权方法，构造的渐消记忆时变噪声统计估值器和q^k=(1-dk)q^k-1+dk[x^k|k-1mΣi=1mf(Xi,k-1|k-1)],]]>Q^k=(1-dk)Q^k-1+dk[KkγkγkTKkT+Pk|k-1mΣi=1mXi,k|k-1zi,k|k-1T+xk|k-1^z^k|k-1T],]]>r^k=(1-dk)r^k-1+dk[zk-1mΣi=1mh(Xi,k|k-1)],]]>R^k=(1-dk)R^k-1+dk[γkγkT-1mΣi=1mzi,k|k-1zi,k|k-1T+z^k|k-1z^k|k-1T],]]>其中，d_k=(1-b)/(1-b^k)，b为遗忘因子，且满足0<b<1；γ_k为k时刻的残差；（5）实时估计和修正噪声：利用步骤（4）中所描述的渐消记忆时变噪声统计估值器和对系统噪声Q和量测噪声R进行实时估计和修正。