一种改进型无损卡尔曼滤波室内动目标定位方法

摘要

本发明公开了一种改进型无损卡尔曼滤波室内动目标定位方法，针对室内定位中采用传统无损卡尔曼滤波方法定位精度不高、实时性较差的问题，本发明提出一种运用比例因子动态调整噪声协方差矩阵的迭代算法来改进传统无损卡尔曼滤波，即在滤波过程中，不断利用上一时刻的滤波值代入时间更新方程，建立起对当前状态的先验估计，进而推算出当前时刻状态变量和误差协方差的估计值，并在此基础上完成对观测噪声和系统噪声的更新，以保证算法的收敛性和稳定性。

主权项

一种改进型无损卡尔曼滤波室内动目标定位方法，其特征在于，包括以下步骤：1)、根据下式对动力学系统进行数学建模，其中，所建的数学模型中动态目标初始运动状态为x，协方差阵为P_x：x_k＝f(x_k‑1,u_k‑1,w_k‑1)，此式表示系统的动态模型，其中x_k∈Rⁿ为n维的随机状态向量，表示目标在k时刻的状态，x_k‑1则表示目标在k‑1时刻的状态，u_k‑1为控制量；w_k‑1表示n维的系统噪声向量；z_k＝h(x_k,v_k)，此式表示动态系统的观测模型，z_k为m维的观测向量，v_k表示m维的观测噪声向量；其中，w_k‑1和v_k为相互独立且均值为零的正态白噪声，协方差矩阵分别为Q_k‑1，R_k，f(·)为有界的非线性状态方程；h(·)为有界的非线性观测方程；2)、根据动态目标初始运动状态向量x和协方差阵P_x选择sigma点采样策略，得到初始状态向量x的sigma点集，并由下式：${w_i}^{\left(m\right)}={w_i}^{\left(c\right)}=\left\{\begin{array}{cc}\frac{\kappa }{n+\kappa }& i=0\\ \frac{\kappa }{2(n+\kappa )}& i\ne 0\end{array}\right.$计算得到相应的均值权重w_i^(m)和方差权重w_i^(c)，其中上标m和c分别用来区分均值权重和方差权重；参数κ用来确定给定分布的高阶矩信息，n为初始状态向量x的维数；3)、均值附近的第i个sigma点到中心点的距离矩阵表达式为其中P_x为任意时刻状态向量x的协方差阵，其中，为矩阵平方根的第i列，采样点数为2n+1，在k‑1时刻n维状态向量x_k‑1的均值为协方差阵为P_k‑1，产生的2n+1个n维列向量χ_k,k‑1(:,i)：${\chi }_{k-1}(:,i)=\left\{\begin{array}{cc}{\bar{x}}_{k-1}& i=0\\ {\bar{x}}_{k-1}-{\left[\sqrt{(n+\kappa )·P_{k-1}}\right]}_i& i=1,...,n\\ {\bar{x}}_{k-1}+{\left[\sqrt{(n+\kappa )·P_{k-1}}\right]}_i& i=1,...,n\end{array}\right.$其中，χ_k,k‑1(:,i)表示向量χ_k‑1的第i列，将输入变量的sigma点集中每一个采样点代入系统状态方程f(·)进行非线性变换，得到变换后的sigma点集为n×(2n+1)维的矩阵：χ_k,k‑1＝f(χ_k‑1,u_k‑1,w_k‑1)对变换后的sigma点集根据下式进行加权处理，得到一步预测状态${\hat{x}}_{k,k-1}=\underset{i=0}{\overset{l}{\Sigma }}{w_i}^{\left(m\right)}·{\chi }_{k,k-1}(:,i)$其中，χ_k,k‑1(:,i)为矩阵χ_k,k‑1的第i列；使用同样的方法得到一步预测方差阵P_k,k‑1：$P_{k,k-1}=\underset{i=0}{\overset{l}{\Sigma }}[{w_i}^{\left(c\right)}·({\chi }_{k,k-1}(:,i)-{\hat{x}}_{k,k-1})·{({\chi }_{k,k-1}(:,i)-{\hat{x}}_{k,k-1})}^T]+Q_k$其中，χ_k,k‑1(:,i)是矩阵χ_k,k‑1的第i列；Q_k为w_k的协方差矩阵；根据一步预测状态和一步预测方差阵P_k,k‑1，根据下式产生新的sigma点集：${{\chi }_{k,k-1}}^{\left(z\right)}(:,i)=\left\{\begin{array}{cc}{\hat{x}}_{k,k-1}& i=0\\ {\hat{x}}_{k,k-1}-{\left[\sqrt{(n+\kappa )·P_{k,k-1}}\right]}_i& i=1,...,n\\ {\hat{x}}_{k,k-1}+{\left[\sqrt{(n+\kappa )·P_{k,k-1}}\right]}_i& i=n+1,...,2n\end{array}\right.$其中，χ_k,k‑1^(z)表示由一步预测状态得到的sigma点集，χ_k,k‑1^(z)(:,i)为χ_k,k‑1^(z)的第i列，将χ_k,k‑1^(z)代入非线性观测方程h(·)进行非线性变换：z_k＝h(χ_k,k‑1^(z),v_k)使用加权求和计算得到系统的预测观测值${\hat{z}}_k=\underset{i=0}{\overset{l}{\Sigma }}{w_i}^{\left(m\right)}·z_k\left(i\right)$其中，z_k(i)为向量z_k中第i个元素；4)、定义观测残差序列为${ϵ}_k=z_k-h({\hat{x}}_{k,k},v_k);$定义观测新息序列为$d_k=z_k-h({\hat{x}}_{k,k-1},v_k);$在步骤2)和步骤3)中，对观测残差序列值ε_k与系统噪声协方差矩阵Q_k的第一基准值进行实时比较，对观测新息序列值d_k与观测噪声协方差矩阵R_k的第二基准值进行实时比较；其中，Q_k为系统噪声向量w_k的系统噪声协方差矩阵，R_k为观测噪声向量v_k的观测噪声协方差矩阵：取系统噪声协方差矩阵Q_k的第一基准值为maxchange，滤波更新后将计算得到的观测残差序列ε_k与第一基准值maxchange进行比较，若观测残差序列ε_k大于所述第一基准值，则根据下式更新原系统噪声协方差矩阵Q_k：Q_k＝maxchange/2取观测噪声协方差矩阵R_k的第二基准值为dispersion，与观测新息序列d_k进行比较，若观测新息序列d_k大于所述第二基准值，则引入比例因子sigmasquare对原观测噪声协方差矩阵R_k进行调整：R_k＝dispersion·sigmasquare其中，sigmasquare为比例因子，所述比例因子根据具体室内环境及观测目标取值；5)、更新状态向量：${\hat{x}}_{k,k}={\hat{x}}_{k,k-1}+K_k·(z_k-{\hat{z}}_k)$其中为x_k的最优预测，为z_k最优预测，K_k称为卡尔曼滤波增益，其更新值由下式得到：$K_k=P_{x_k{z}_k}·{P_{z_k}}^{-1}$其中，协方差由下式得到：$P_{x_k{z}_k}=\underset{i=0}{\overset{l}{\Sigma }}{w_i}^{\left(c\right)}·[{\chi }_{k,k-1}(:,i)-{\hat{x}}_{k,k-1}]·{(z_k\left(i\right)-{\hat{z}}_k)}^T$系统量测输出变量的方差阵由下式得到：$P_{z_k}=\underset{i=0}{\overset{l}{\Sigma }}{w_i}^{\left(c\right)}·(z_k\left(i\right)-{\hat{z}}_k)·{(z_k\left(i\right)-{\hat{z}}_k)}^T+R_k$更新状态后验方差阵：$P_k=P_{k,k-1}-K_k·P_{z_k}·{K_k}^T.$