一种平方根单形容积卡尔曼滤波方法

摘要

本发明提供了一种平方根单形容积卡尔曼滤波方法，依次计算系统预测状态及其协方差矩阵；计算预测量测及其协方差矩阵；计算滤波增益；更新得到系统状态及其协方差矩阵。本发明通过引入平方根滤波技术消除了由计算机舍入误差带来的协方差矩阵不正定问题，提高了滤波方法的鲁棒性，采用的单形采样方法可以大幅减少运算量，相对传统的滤波方法也提高了滤波精度。

主权项

一种平方根单形容积卡尔曼滤波方法，其特征在于包括下述步骤：步骤1、给定系统初始状态均值和误差协方差矩阵P_0|0＝P(0)，然后计算单形采样的容积基点ξ_i及其相应的权值ω_i；${\xi }_i=\left\{\begin{array}{cc}{\left[0\right]}_n,& i=1\\ \sqrt{n+2}{a}_j,& 2\le i\le n+2\\ -\sqrt{n+2}a,& n+3\le i\le 2n+3\\ \sqrt{n+2}{b}_j,& 2n+4\le i\le (n^2+5n+3)/2\\ -\sqrt{n+2}{b}_j,& (n^2+5n+5)/2\le i\le n^2+3n+3\end{array}\right.$${\omega }_i=\left\{\begin{array}{cc}\frac{2}{n+2},& i=1\\ \frac{(7-n)n}{2{(n+1)}^2{(n+2)}^2},& 2\le i\le 2n+3\\ \frac{2{(n-1)}^2}{n{(n+1)}^2{(n+2)}^2},& 2n+4\le i\le n^2+3n+3\end{array}\right.$其中，a_j是代表一组n维列向量，a_j＝[a_j,1,a_j,2,…,a_j,n]^T，j＝1,2,…,n+1，$a_{j,t}=\left\{\begin{array}{cc}-\sqrt{\frac{n+1}{n(n-t+2)(n-t+1)}},& t<j\\ \sqrt{\frac{(n+1)(n-j+1)}{n(n-j+2)}},& t=j,1\le t\le n\\ 0,& t>j\end{array}\right.;$$\left\{b_j\right\}=\{\sqrt{\frac{n}{2(n-1)}}(a_j+a_l):j<l\},$其中l＝1,2,…,n+1；步骤2、时间更新，包括以下步骤：2.1分解状态协方差矩阵P_k‑1|k‑1得到S_k‑1|k‑1＝chol(P_k‑1|k‑1)，chol是指Cholesky矩阵分解；2.2计算状态容积点${\chi }_{i,k-1|k-1}=S_{k-1|k-1}·{\xi }_i+{\hat{x}}_{k-1|k-1}$并传播，得到${\chi }_{i,k|k-1}^{*}=f\left({\chi }_{i,k-1|k-1}\right);$2.3计算预测状态及其协方差矩阵$P_{k|k-1}=\underset{i=1}{\overset{m}{\Sigma }}{\omega }_i{\chi }_{i,k|k-1}{\chi }_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1};$步骤3、量测更新，包括以下步骤：3.1分解预测状态协方差矩阵得到S_k|k‑1＝chol(P_k|k‑1)；3.2计算预测状态容积点并传播，得到预测量测容积点3.3计算预测量测及其协方差矩阵$P_{\mathrm{zz}}=\underset{i=1}{\overset{m}{\Sigma }}{\omega }_i\left(z_{i,k|k-1}{-\hat{z}}_{k|k-1}\right){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T+R_k;$3.4计算预测状态和预测量测的协方差矩阵$P_{\mathrm{xz}}=\underset{i=1}{\overset{m}{\Sigma }}{\omega }_i({\chi }_{i,k|k-1}-{\hat{x}}_{k|k-1}){(z_{i,k|k-1}-{\hat{z}}_{k|k-1})}^T,$求取滤波增益$G_k=P_{\mathrm{xz}}·P_{\mathrm{zz}}^{-1};$3.5更新得到系统状态${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+G_k(z_k-{\hat{z}}_{k|k-1})$及其协方差矩阵$P_k=P_{k|k-1}-G_k{P}_{\mathrm{zz}}{G}_k^T;$步骤4：判断滤波算法是否继续执行，如果是，返回步骤2；否则，结束算法。