[发明专利]未知测量噪声方差的平方根高阶容积卡尔曼滤波方法

摘要

本发明属于非线性系统的滤波领域，特别涉及一种处理含有未知测量噪声方差的平方根高阶容积卡尔曼滤波方法。该方法用于处理未知测量噪声方差的平方根高阶容积Kalman滤波，以HCKF为基础，首先利用QR分解，Cholesky因子更新和高效最小二乘法等矩阵分解技术，提高了滤波方法的运行效率和数值稳定性。并在SHCKF基础上，通过采用Sage‑Husa估计器实时估算量测噪声的统计特性，有效解决了测量噪声方差位置的非线性系统滤波问题。

主权项

未知测量噪声方差的平方根高阶容积卡尔曼滤波方法，其特征在于：设非线性动态系统的状态空间模型为：x(k+1)＝f(x(k))+w(k)z(k)＝h(x(k))+v(k)上式中，x(k)∈Rn为目标状态，z(k)∈Rm表示测量值；f:Rn→Rn为非线性状态演化过程，h:Rn→Rm为相应的非线性测量映射；过程噪声w(k)∈Rn是均值为零的高斯白噪声，其方差为Q(k)；测量噪声v(k)∈Rm是均值为零的高斯白噪声，但方差R(k)时变未知；假设系统模型中过程噪声与测量噪声互不相关；系统的初始状态x(0)均值为x0，方差为P0，且独立于w(k)和v(k)；基于上述系统模型，该方法具体步骤是：步骤1设置滤波初始条件：S0＝chol(P0)，步骤2预测过程；1)计算高阶容积点xi(k-1|k-1)=S(k-1|k-1)ξi+x^(k-1|k-1)---(1)]]>其中，S(k‑1|k‑1)为方差P(k‑1|k‑1)的Cholesky分解因子，点集{ξi}由下式确定：ξi=[0,...,0]T,i=0n+2·ϵi+,i=1,...,n(n-1)2-n+2·ϵi-n(n-1)/2+,i=n(n-1)2+1,...,n(n-1)n+2·ϵi-n(n-1)-,i=n(n-1)+1,...,3n(n-1)2-n+2·ϵi-3n(n-1)/2-,i=3n(n-1)2+1,...,2n(n-1)n+2·ei-2n(n-1),i=2n(n-1)+1,...,n(2n-1)-n+2·ei-n(2n-1),i=n(2n-1)+1,...,2n2]]>其中，ei为n阶单位向量，且其第i个元素为1；点集和由下式给出：而相应的权值系数ωi为：ωi=2n+2,i=01(n+2)2,i=1,...,2n(n-1)4-n2(n+2)2,i=2n(n-1)+1,...,2n2]]>2)计算传播后的容积点xi*(k|k-1)=f(xi(k-1|k-1))---(2)]]>3)状态预测估计值x^(k|k-1)=Σi=02n2ωixi*(k|k-1)---(3)]]>4)计算预测误差方差阵的平方根S(k|k-1)=qr[ω0(x0*(k|k-1)-x^(k|k-1))...ω2n2(x2n2*(k|k-1)-x^(k|k-1)),Q(k-1)]---(4)]]>其中，qr(·)表示对矩阵进行QR分解，表示矩阵Q(k‑1)的Cholesky分解因子；步骤3更新过程1)计算容积点Si(k|k-1)=S(k|k-1)ξi+x^(k|k-1)---(5)]]>2)计算传播后的容积点zi(k|k‑1)＝h(xi(k|k‑1))      (6)3)测量预测z^(k|k-1)=Σi=02n2ωizi(k|k-1)---(7)]]>4)计算互协方差阵与增益阵Pxz(k|k-1)=Σi=02n2ωi[xi(k|k-1)-x^(k|k-1)]×[zi(k|k-1)-z^(k|k-1)]T---(8)]]>5)在设计状态估计算法时需要在线估计v(k)的方差R^(k)=[1-d(k-1)]R^(k-1)+d(k-1)[z~(k)×z^T(k)-H(k,k-1)P(k|k-1)HT(k,k-1)]---(9)]]>其中：Pxz(k|k‑1)＝P(k|k‑1)HT(k,k‑1)H(k,k-1)=PxzT(k|k-1)/(ST(k|k-1)S(k|k-1))]]>d(k)＝(1‑b)/(1‑bk+1)，b为遗忘因子，b用来限制滤波器的记忆长度；为测量新息，H(k,k‑1)为线性化后的观测矩阵；即：R^(k)=[1-d(k-1)]R^(k-1)+d(k-1)×[z~(k)z^T(k)-PxzT(k|k-1)×(ST(k,k-1)S(k,k-1))-1×Pxz(k|k-1)]---(10)]]>6)计算新息协方差阵的平方根Szz(k|k-1)=qr[ω0(z0(k|k-1)-z^(k|k-1))...ω2n2(z2n2(k|k-1)-z^(k|k-1))R^(k)]---(11)]]>7)计算增益阵K(k)K(k)=[Pxz(k|k-1)/SzzT(k|k-1)]/Szz(k|k-1)---(12)]]>8)估计状态及其协方差阵的平方根x^(k|k)=x^(k|k-1)+K(k)[z(k)-z^(k|k-1)]S(k|k)=cholupdate[S(k|k-1),K(k)Szz(k|k-1),-1]---(13).]]>