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

Abstract

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

Description

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

技术领域

本发明属于室内定位系统中不规则运动目标的实时跟踪领域，具体涉及一种改进型无损卡尔曼滤波室内动目标定位方法。

背景技术

传统的滤波技术是基于处理确定性信号的滤波问题而提出的，但在处理随机系统状态估计问题中，有用信号和噪声干扰都是多维非平稳随机过程，为解决实时滤波以及非平稳信号等问题，Rudolph E.Kalman于1960年提出了卡尔曼滤波理论。

最初提出的卡尔曼滤波算法仅仅适用于线性观测的线性系统，一些学者针对这一问题提出了适用于非线性条件下的扩展卡尔曼滤波算法。该算法对非线性系统的系统方程或观测方程进行泰勒展开，并取其一阶近似项。这样的处理不可避免地引入了线性化误差，降低了模型的准确性，无法保证估计精度。

为了解决扩展卡尔曼滤波中存在的问题，Simon J.Julier和Jeffrey K.Uhlmann提出了一种新的适用于非线性系统的无损卡尔曼滤波器（Unscented Kalman Filtering,UKF）。无损卡尔曼滤波算法的核心思想是：将高斯变量经非线性变换后的概率分布用高斯分布来近似。当系统具有非线性特征时，在相同仿真条件下，无损卡尔曼滤波算法较之扩展卡尔曼滤波算法能为卡尔曼滤波器提供更好的非线性均值和方差的近似解，因而对系统状态的估计更为准确，定位精度更高，且计算量更小。

在采用无损卡尔曼滤波算法进行室内动目标定位时，抗干扰能力较差的滤波器容易受到系统参数的漂移、噪声估计器性能的改变、以及系统状态的跳变等干扰，并因此而很快失去精确性和稳定性，甚至导致滤波器发散；同时，由于动态系统的数学模型及噪声统计特性只是对实际物理模型的近似，因此在实际应用中，滤波误差始终是存在的。

因此，需要一种新的无损卡尔曼滤波室内动目标定位方法以解决上述问题。

发明内容

发明目的：本发明针对现有技术中的无损卡尔曼滤波算法进行室内动目标定位的缺陷，提供一种能够有效抵抗噪声干扰并同时保证精确性及稳定性的改进型无损卡尔曼滤波室内动目标定位方法。

技术方案：为解决上述技术问题，本发明的改进型无损卡尔曼滤波室内动目标定位方法采用如下技术方案：

一种改进型无损卡尔曼滤波方法，其特征在于，包括以下步骤：

（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）、定义观测残差序列为

定义观测新息序列为 $d_k=z_k-h({\hat{x}}_{k,k-1},v_k);$

对观测残差序列值ε_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为比例因子，所述比例因子根据具体室内环境及观测目标取值。

有益效果：本发明的改进型无损卡尔曼滤波方法通过构造可依据系统噪声和观测噪声自适应调整的比例因子：当系统处于非机动状态时，比例因子取值变小；当系统发生快速机动时，比例因子可迅速增大，从而增大系统的噪声协方差和观测噪声协方差，达到快速实时调整系统卡尔曼增益值，提高滤波性能的目的。通过简单而灵活的噪声比例调整，能够实现噪声的在线调整和估计，进而可保证探测数据的快速收敛，并以此改善传统无损卡尔曼滤波的实时跟踪性能，能够比传统方法更好地实现对无规则运动目标的实时轨迹跟踪。

本发明还公开了一种改进型无损卡尔曼滤波室内动目标定位方法，采用如下的技术方案：

一种改进型无损卡尔曼滤波室内动目标定位方法，其特征在于，包括以下步骤：

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{\mathrm{\κ}}{n+\mathrm{\κ}}& i=0\\ \frac{\mathrm{\κ}}{2(n+\mathrm{\κ})}& i\≠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)：

${\mathrm{\χ}}_{k-1}(:,i)=\left\{\begin{array}{cc}{\stackrel{\‾}{x}}_{k-1}& i=0\\ {\stackrel{\‾}{x}}_{k-1}-[\sqrt{(n+\mathrm{\κ})\·P_{k-1}}{]}_i& i=1,...,n\\ {\stackrel{\‾}{x}}_{k-1}+[\sqrt{(n+\mathrm{\κ})\·P_{k-1}}{]}_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}{\mathrm{\Σ}}}{w_i}^{\left(m\right)}\·{\mathrm{\χ}}_{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}{\mathrm{\Σ}}}[{w_i}^{\left(c\right)}\·({\mathrm{\χ}}_{k,k-1}(:,i)-{\hat{x}}_{k,k-1})\·{({\mathrm{\χ}}_{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点集：

${{\mathrm{\χ}}_{k,k-1}}^{\left(z\right)}(:,i)=\left\{\begin{array}{cc}{\hat{x}}_{k,k-1}& i=0\\ {\hat{x}}_{k,k-1}-[\sqrt{(n+\mathrm{\κ})\·P_{k,k-1}}{]}_i& i=1,...,n\\ {\hat{x}}_{k,k-1}+[\sqrt{(n+\mathrm{\κ})\·P_{k,k-1}}{]}_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)

使用加权求和计算得到系统的预测观测值

${\mathrm{\ϵ}}_k=z_k-h({\hat{x}}_{k,k},v_k);$

其中，z_k(i)为向量z_k中第i个元素；

4）、定义观测残差序列为

定义观测新息序列为 $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}{\mathrm{\Σ}}}{w_i}^{\left(c\right)}\·[{\mathrm{\χ}}_{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}{\mathrm{\Σ}}}{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.$

更进一步的，步骤2）中所述的sigma点采样策略为对称采样策略。

发明原理：由于在迭代过程中，某些变量的计算过程涉及到系统噪声协方差矩阵Q与观测噪声协方差矩阵R，其值将影响当前处理信息和上一次观测信息之间的权重，而权重信息又直接影响状态更新后的滤波值。因此，系统噪声协方差矩阵Q与观测噪声协方差矩阵R取值的误差，将直接导致滤波器滤波的发散。为解决此问题，传统方法中是利用先验知识及经验分析，将Q与R的取值固定为某一定值，且不随外界环境变化进行实时调整，这显然会降低滤波性能。本发明提出一种基于比例因子动态调整噪声协方差矩阵的卡尔曼滤波增益计算方法，能够有效地克服上述障碍、提高无损卡尔曼滤波性能。对观测残差序列值、观测新息序列值与设定的参数进行实时比较，当观测残差值和观测新息值大于设定参数值时，说明载体周围环境或系统运动状态已发生剧烈变化，系统观测会出现较大波动，此时引入比例因子使系统快速收敛，从而提高系统自适应性能。

由于观测残差序列由当前时刻状态更新后的滤波值代入观测方程求得，是当前时刻经观测向量修正后的状态差值；而观测新息序列是由当前时刻的预测估计状态推算得到的，即观测新息序列使用的是未经修改的，用状态模型计算得到的状态估计值，带有噪声不定性所造成的误差，是当前时刻未经观测向量进行修正的状态差值。因此观测残差序列带有更多的更新信息，可用来调整系统噪声；而观测新息序列更能反映系统的扰动，可用来调整观测噪声。

有益效果：本发明的改进型无损卡尔曼滤波室内动目标定位方法通过构造可依据系统噪声和观测噪声自适应调整的比例因子：当系统处于非机动状态时，比例因子取值变小；当系统发生快速机动时，比例因子可迅速增大，从而增大系统的噪声协方差和观测噪声协方差，达到快速实时调整系统卡尔曼增益值，提高滤波性能的目的。通过简单而灵活的噪声比例调整，能够实现噪声的在线调整和估计，进而可保证探测数据的快速收敛，并以此改善传统无损卡尔曼滤波的实时跟踪性能，能够比传统方法更好地实现对无规则运动目标的实时轨迹跟踪。

附图说明

图1是本发明的室内定位无损卡尔曼滤波室内目标定位方法的流程图；

图2是本发明的室内定位无损卡尔曼滤波室内目标定位方法的更新模块流程图；

图3是本发明的室内定位无损卡尔曼滤波室内目标定位方法的求解sigma点模块流程图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

请参阅图1、图2和图3所示，本发明的改进型无损卡尔曼滤波室内动目标定位方法，包括以下步骤：

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点采样策略，所述的sigma点采样策略为对称采样策略。得到初始状态向量x的sigma点集，并由下式：

${w_i}^{\left(m\right)}={w_i}^{\left(c\right)}=\left\{\begin{array}{cc}\frac{\mathrm{\κ}}{n+\mathrm{\κ}}& i=0\\ \frac{\mathrm{\κ}}{2(n+\mathrm{\κ})}& i\≠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)：

${\mathrm{\χ}}_{k-1}(:,i)=\left\{\begin{array}{cc}{\stackrel{\‾}{x}}_{k-1}& i=0\\ {\stackrel{\‾}{x}}_{k-1}-[\sqrt{(n+\mathrm{\κ})\·P_{k-1}}{]}_i& i=1,...,n\\ {\stackrel{\‾}{x}}_{k-1}+[\sqrt{(n+\mathrm{\κ})\·P_{k-1}}{]}_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}{\mathrm{\Σ}}}{w_i}^{\left(m\right)}\·{\mathrm{\χ}}_{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}{\mathrm{\Σ}}}[{w_i}^{\left(c\right)}\·({\mathrm{\χ}}_{k,k-1}(:,i)-{\hat{x}}_{k,k-1})\·{({\mathrm{\χ}}_{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点集：

${{\mathrm{\χ}}_{k,k-1}}^{\left(z\right)}(:,i)=\left\{\begin{array}{cc}{\hat{x}}_{k,k-1}& i=0\\ {\hat{x}}_{k,k-1}-[\sqrt{(n+\mathrm{\κ})\·P_{k,k-1}}{]}_i& i=1,...,n\\ {\hat{x}}_{k,k-1}+[\sqrt{(n+\mathrm{\κ})\·P_{k,k-1}}{]}_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}{\mathrm{\Σ}}}{w_i}^{\left(m\right)}\·z_k\left(i\right)$

其中，z_k(i)为向量z_k中第i个元素；

4）、定义观测残差序列为

定义观测新息序列为 $d_k=z_k-h({\hat{x}}_{k,k-1},v_k);$

在步骤2）和步骤3）中，对观测残差序列值ε_k与系统噪声协方差矩阵Q_k的第一基准值进行实时比较，对观测新息序列值d_k与观测噪声协方差矩阵R_k的第二基准值进行实时比较；

由于在迭代过程中，某些变量的计算过程涉及到系统噪声协方差矩阵Q与观测噪声协方差矩阵R，其值将影响当前处理信息和上一次观测信息之间的权重，而权重信息又直接影响状态更新后的滤波值。因此，系统噪声协方差矩阵Q与观测噪声协方差矩阵R取值的误差，将直接导致滤波器滤波的发散。为解决此问题，传统方法中是利用先验知识及经验分析，将Q与R的取值固定为某一定值，且不随外界环境变化进行实时调整，这显然会降低滤波性能。本发明提出一种基于比例因子动态调整噪声协方差矩阵的卡尔曼滤波增益计算方法，能够有效地克服上述障碍、提高无损卡尔曼滤波性能。对观测残差序列值、观测新息序列值与设定的参数进行实时比较，当观测残差值和观测新息值大于设定参数值时，说明载体周围环境或系统运动状态已发生剧烈变化，系统观测会出现较大波动，此时引入比例因子使系统快速收敛，从而提高系统自适应性能。

由于观测残差序列由当前时刻状态更新后的滤波值代入观测方程求得，是当前时刻经观测向量修正后的状态差值；而观测新息序列是由当前时刻的预测估计状态推算得到的，即观测新息序列使用的是未经修改的，用状态模型计算得到的状态估计值，带有噪声不定性所造成的误差，是当前时刻未经观测向量进行修正的状态差值。因此观测残差序列带有更多的更新信息，可用来调整系统噪声；而观测新息序列更能反映系统的扰动，可用来调整观测噪声。

取系统噪声协方差矩阵Q_k的第一基准值为maxchange，滤波更新后将计算得到的观测残差序列ε_k与第一基准值maxchange进行比较，若观测残差序列ε_k大于所述第一基准值，说明此时系统出现较大跳动，需更新原系统噪声协方差矩阵Q_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}{\mathrm{\Σ}}}{w_i}^{\left(c\right)}\·[{\mathrm{\χ}}_{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}{\mathrm{\Σ}}}{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.$

本发明主要由三个模块构成：更新模块，sigma点求解模块，以及无损卡尔曼滤波模块。

下面分别说明三个主要模块的具体流程。

（1）更新模块

更新模块如附图2所示，主要包括启动部分，UKF滤波部分，参数更新部分。

启动部分：从测量输入文件中读入数据后，首先将其送入预先设定的矩阵中，并以该矩阵作为观测数据代入滤波过程进行运算；由于UKF是一种递推算法，启动时必须先给变量x和P_x赋初值，本算法使用第一次观测获得的观测值作为x的初始值，对于P_x则可以根据经验对其赋初值。

UKF滤波部分：调用求解sigma点模块以及UKF滤波模块对输入数据进行UKF滤波，利用上一时刻的均值x和协方差P_x计算出当前的滤波值。

参数更新部分：最后在测试部分进行参数更新（包括均值x，协方差P_x，系统噪声协方差矩阵Q以及观测噪声协方差矩阵R），将均值x与协方差P_x作为下一次滤波求解sigma点的输入变量，并向输出文件输出滤波数据。

（2）sigma点求解模块

如附图3所示，得到了x和P_x的初始值之后，随即代入sigma点求解函数中进行UT（Unscented Transform）变换，得到滤波过程所需要的sigma点，均值权重w_i ^(m)以及方差权重w_i ^(c)。

（3）UKF滤波模块

将上一过程中得到的sigma点，均值的权重w_i ^(m)以及方差的权重w_i ^(c)代入UKF（Unscented Kalman Filtering,UKF）函数进行滤波。

在UKF滤波过程中，由系统方程对sigma点集进行非线性变换，再对变换后的sigma点集进行加权处理，从而得到一步预测状态和一步预测方差阵，再将一步预测状态值代入观测方程进行非线性变换，并进行加权处理得到系统的预测观测值，将其代入测量更新方程计算出滤波增益，更新状态估计值以及更新误差方差阵之后，还要进一步计算出残差序列和更新序列的更新值，并将其与系统参数进行比较，若满足条件（即系统运动状态出现较大跳变时），则运用比例因子动态调整噪声协方差矩阵的方法对系统噪声和观测噪声进行更新。

仿真验证

下面以对作抛物线运动的动态目标进行实时追踪为例来介绍并验证本发明所提出的改进型无损卡尔曼滤波室内动目标定位方法。

考虑一个在二维平面内运动的质点，假设其在某一时刻k的位移、速度和加速度可用向量x_k＝[s_x,s_y,v_x,v_y,a_x,a_y]表示。该质点在水平方向x作匀速直线运动，在垂直方向y做匀加速直线运动。两方向上均有加性系统噪声w_k-1，则在笛卡尔坐标系下该质点的运动状态方程为：

x_k=f(x_k-1,w_k-1)

其中，w_k-1为加性噪声，f(·)对w_k-1不产生作用，而对x_k-1的作用可以表示为状态转移矩阵F：

$F=\left[\begin{array}{cccccc}1& 0& t& 0& t^2/2& 0\\ 0& 1& 0& t& 0& t^2/2\\ 0& 0& 1& 0& t& 0\\ 0& 0& 0& 1& 0& t\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]$

其中，t为计时零时刻到k时刻所用的时间，x_k-1经状态转移矩阵F作用后，可以表示为：

$\left\{\begin{array}{c}{s}_{x,k}=s_{x,k-1}+v_{x,k-1}\·t+a_{x,k-1}\·t^2/2\\ s_{y,k}=s_{y,k-1}+v_{y,k-1}\·t+a_{y,k-1}\·t^2/2\\ v_{x,k}=v_{x,k-1}+a_{x,k-1}\·t\\ v_{y,k}=v_{y,k-1}+a_{y,k-1}\·t\\ a_{x,k}=a_{x,k-1}\\ a_{y,k}=a_{y,k-1}\end{array}\right.$

假设目标位置为（0，0）的基站对质点直接进行水平位移和垂直位移测量测量，并在测量中具有加性观测噪声v_k，则在笛卡尔坐标系下该质点的观测方程为：

z_k＝h(x_k,v_k)

其中，h(·)为非线性观测方程，其对x_k作用为转移矩阵H,由于v_k为加性噪声，故对v_k不产生作用：

$H=\left[\begin{array}{c}1\\ 1\end{array}\right]$

分别采用传统UKF算法及采用基于比例因子动态调整噪声协方差矩阵技术的改进UKF算法对质点的位移、速度及加速度等误差信息进行估计。w_k-1和v_k分别为系统噪声和观测噪声，均假设为高斯白噪声，因受内外部环境因素的影响，它们的统计特性具有时变性和随机性。同时，取系统噪声协方差矩阵初始值为

$Q=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 0.01& 0& 0& 0\\ 0& 0& 0& 0.01& 0& 0\\ 0& 0& 0& 0& 0.0001& 0\\ 0& 0& 0& 0& 0& 0.0001\end{array}\right]$

取观测噪声协方差矩阵初始值为

$R=\left[\begin{array}{cc}25& 0\\ 0& 0.0001\end{array}\right]$

经过以上假设后，分别对数据使用传统UKF算法和估计比例因子动态调整噪声协方差矩阵的改进UKF算法进行滤波。对于传统UKF算法，其滤波过程中采用定常噪声协方差矩阵；而改进UKF算法则通过式(1)与式(2)分别对系统噪声和观测噪声进行修正。

质点运动的水平方向位移分量x的滤波值与水平方向位移分量y的滤波值比较如下：

通过表中对传统无损卡尔曼滤波算法和采用基于比例因子动态调整噪声协方差矩阵技术的改进型无损卡尔曼算法的滤波数据的比较，可以清晰地看出：对于传统无损卡尔曼滤波算法，尽管滤波开始的一段时间内数据是收敛的，但随着时间的推移，滤波数据开始趋于发散，并且发散情况会越来越严重，累积到一定时间后非线性滤波将完全失效，因此采用传统UKF算法并不能够很好地跟踪真实的轨迹曲线。也就是说，与真实值相比，传统UKF滤波器产生了较为显著的误差滤波值；而对于采用基于比例因子动态调整噪声协方差矩阵技术的改进UKF算法，不仅在整个滤波过程中数据都趋于收敛，而且进一步对比同一时刻的数据可以发现，改进UKF算法在每一处估值点都能够得到比传统UKF算法更为精确的滤波值，也正是由于比例因子的引入，使得改进后的滤波过程更加平滑，误差更小。

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

Claims (3)

1.一种改进型无损卡尔曼滤波方法，其特征在于，包括以下步骤：

（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）、定义观测残差序列为

定义观测新息序列为 $d_k=z_k-h({\hat{x}}_{k,k-1},v_k);$

对观测残差序列值ε_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为比例因子，所述比例因子根据具体室内环境及观测目标取值。

2.一种改进型无损卡尔曼滤波室内动目标定位方法，其特征在于，包括以下步骤：

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{\mathrm{\κ}}{n+\mathrm{\κ}}& i=0\\ \frac{\mathrm{\κ}}{2(n+\mathrm{\κ})}& i\≠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)：

${\mathrm{\χ}}_{k-1}(:,i)=\left\{\begin{array}{cc}{\stackrel{\‾}{x}}_{k-1}& i=0\\ {\stackrel{\‾}{x}}_{k-1}-[\sqrt{(n+\mathrm{\κ})\·P_{k-1}}{]}_i& i=1,...,n\\ {\stackrel{\‾}{x}}_{k-1}+[\sqrt{(n+\mathrm{\κ})\·P_{k-1}}{]}_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}{\mathrm{\Σ}}}{w_i}^{\left(m\right)}\·{\mathrm{\χ}}_{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}{\mathrm{\Σ}}}[{w_i}^{\left(c\right)}\·({\mathrm{\χ}}_{k,k-1}(:,i)-{\hat{x}}_{k,k-1})\·{({\mathrm{\χ}}_{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点集：

${{\mathrm{\χ}}_{k,k-1}}^{\left(z\right)}(:,i)=\left\{\begin{array}{cc}{\hat{x}}_{k,k-1}& i=0\\ {\hat{x}}_{k,k-1}-[\sqrt{(n+\mathrm{\κ})\·P_{k,k-1}}{]}_i& i=1,...,n\\ {\hat{x}}_{k,k-1}+[\sqrt{(n+\mathrm{\κ})\·P_{k,k-1}}{]}_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}{\mathrm{\Σ}}}{w_i}^{\left(m\right)}\·z_k\left(i\right)$

其中，z_k(i)为向量z_k中第i个元素；

4）、定义观测残差序列为

定义观测新息序列为 $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}{\mathrm{\Σ}}}{w_i}^{\left(c\right)}\·[{\mathrm{\χ}}_{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}{\mathrm{\Σ}}}{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.$

3.如权利要求2所述的改进型无损卡尔曼滤波室内动目标定位方法，其特征在于，步骤2）中所述的sigma点采样策略为对称采样策略。

Images