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

权利要求书

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进行实时估计和修正。