CN107797093A

CN107797093A - 基于容积卡尔曼滤波的无线电定位方法

Info

Publication number: CN107797093A
Application number: CN201711001728.6A
Authority: CN
Inventors: 王树磊; 门艳忠; 郭魂; 黄志荣; 江炜
Original assignee: Changzhou Institute of Technology
Current assignee: Changzhou Institute of Technology
Priority date: 2017-10-24
Filing date: 2017-10-24
Publication date: 2018-03-13

Abstract

本发明公开了一种基于容积卡尔曼滤波的无线电定位方法，包括如下步骤：设置平方根容积卡尔曼滤波的初值；完成平方根容积卡尔曼滤波的时间更新；完成平方根容积卡尔曼滤波的量测更新，即可以得到接收机的位置、速度等状态信息；返回第二步，进行迭代滤波。本发明降低了无线电相对定位系统的复杂度和布置成本；不需要对接收机的状态方程及量测方程进行线性化，对接收机状态的估计能够达到二阶泰勒级数的精度；在计算卡尔曼滤波时不需要对矩阵进行求逆运算，因而具有很高的数值稳定性；可以很容易地融合惯性传感器的信息，从而进一步地提高对接收机状态的估计精度。

Description

基于容积卡尔曼滤波的无线电定位方法

技术领域

本发明涉及一种利用无线电波进行相对定位的系统中接收机的位置、速度等状态信息的解算方法。

背景技术

以室内定位为主要代表的无线电相对定位系统具有重大的理论和现实意义。目前，无线电相对定位系统中接收机的状态(包括位置、速度等)解算方法主要分为集中式和分布式两种。前者通过一个中心处理单元，根据基站收集到的其与接收机之间的距离，通常采用三球交汇等原理解算出接收机的位置。浙江大学林志赟(专利申请号201710293070.4)提出一种基于拓展卡尔曼滤波的分布式定位算法，此时，接收机负责解算自己的位置。但采用拓展卡尔曼滤波需要对系统进行线性化，对接收机的状态估计仅能达到一阶泰勒级数的精度；并且在求取卡尔曼增益时需要求取矩阵的逆，在此过程中要求误差方差矩阵始终是正定对称矩阵，在有限字长的数字计算机(特别是嵌入式处理器)中，由于舍入误差的影响，在迭代计算的过程中，误差方差矩阵有可能会丢失正定对称性，导致不能正确的计算出卡尔曼增益，从而导致滤波器运行不稳定。

发明内容

本发明旨在提出一种适用于无线电波相对定位系统的接收机状态(位置、速度等)估计方法，该方法对不需要对接收机的状态方程和观测方程进行线性化处理，对接收机状态的估计能够达到二阶泰勒级数的精度，并且具有良好的数值稳定性。本发明可为无人飞行器、地面机器人等无人智能体提供局部范围内(例如室内)的高精度的位置、速度等信息。

本发明的技术方案如下：

基于容积卡尔曼滤波的无线电定位方法，包括如下步骤：

第一步，设置平方根容积卡尔曼滤波的初值；

第二步，完成平方根容积卡尔曼滤波的时间更新；

第三步，完成平方根容积卡尔曼滤波的量测更新，即可以得到接收机的位置、速度等状态信息，在此之前，需要获取接收机与基站之间的观测信息；

第四步，返回第二步，进行迭代滤波。

进一步，所述第一步，设置平方根容积卡尔曼滤波的初值，包括：接收机的初始位置估计误差方差的平方根初值S(0|0)。

进一步，所述第二步，平方根容积卡尔曼滤波的时间更新包括如下步骤：

步骤2.1：选取容积点，并经过状态方程传播；

步骤2.2：计算非线性状态方程传播后的一阶矩和二阶矩的平方根；

S(k|k-1)＝Tria{[Δx^*(k|k-1) S_Q(k-1)]^T}

式中，“Tria”表示三角分解，典型的三角分解法为QR分解；S_Q(k-1)为k-1次滤波中的状态噪声方差矩阵。

进一步，所述第三步，平方根容积卡尔曼滤波的量测更新包括如下步骤：

步骤3.1：计算新的容积点，并将其通过观测方程进行传播；

Z_i(k|k-1)＝h(χ_i(k|k-1))

步骤3.2：计算经过观测方程传播后的一阶矩和二阶矩的平方根；

S_z(k|k-1)＝Tria{[ΔZ(k|k-1) S_R(k)]^T}

S_R(k-1)为k-1次滤波中的量测噪声方差矩阵。

步骤3.3：计算协方差矩阵；

P_xz(k|k-1)＝Δx(k|k-1)gΔZ^T(k|k-1)

步骤3.4：计算卡尔曼增益K(k)，完成状态估计；

其中，为对接收机状态的估计值，S(k|k)对应估计误差方差矩阵。z(k)为第k次滤波中，接收机通过与基站通信，获取到的其与基站的距离或角度等观测向量。

本发明的有益效果如下：

1、本发明提供一种分布式无线电相对定位系统的接收机状态(包括位置、速度等)估计方法，由于去掉了中心节点，因此降低了无线电相对定位系统的复杂度和布置成本。

2、采用容积卡尔曼滤波估计接收机的状态，该方法是一种非线性滤波算法，不需要对接收机的状态方程及量测方程进行线性化；对接收机状态的估计能够达到二阶泰勒级数的精度。

3、采用平方根形式的容积卡尔曼滤波，并采用三角分解完成矩阵的求平方根运算，在计算卡尔曼滤波时不需要对矩阵进行求逆运算，因而具有很高的数值稳定性。

4、平方根容积卡尔曼滤波可以很容易地融合惯性传感器的信息，从而进一步地提高对接收机状态的估计精度。

附图说明

图1是本发明中平方根容积卡尔曼滤波的流程图。

具体实施方式

下面结合附图对本发明作进一步详细说明。

本发明的基于容积卡尔曼滤波的无线电定位方法包括如下步骤：

第一步，设置平方根容积卡尔曼滤波的初值，包括：接收机的初始位置估计误差方差的平方根初值S(0|0)。

第二步，平方根容积卡尔曼滤波的时间更新。

2.1选取容积点，并经过状态方程传播

2.2计算非线性状态方程传播后的一阶矩和二阶矩的平方根

S(k|k-1)＝Tria{[Δx^*(k|k-1) S_Q(k-1)]^T}

式中，“Tria”表示三角分解，典型的三角分解法为QR分解，下同。S_Q(k-1)为k-1次滤波中的状态噪声方差矩阵。

第三步，平方根容积卡尔曼滤波的量测更新。

3.1计算新的容积点，并将其通过观测方程进行传播

Z_i(k|k-1)＝h(χ_i(k|k-1))

3.2计算经过观测方程传播后的一阶矩和二阶矩的平方根

S_z(k|k-1)＝Tria{[ΔZ(k|k-1) S_R(k)]^T}

S_R(k-1)为k-1次滤波中的量测噪声方差矩阵。

3.3计算协方差矩阵

P_xz(k|k-1)＝Δx(k|k-1)gΔZ^T(k|k-1)

3.4计算卡尔曼增益K(k)，完成状态估计

即为对接收机状态的估计值，S(k|k)对应估计误差方差矩阵。z(k)为第k次滤波中，接收机通过与基站通信，获取到的其与基站的距离或角度等观测向量。由于S为上三角矩阵，因而矩阵的除法可以采用回代法(back substitution algorithm)求取；同理，对除以S^T的运算可以采用前代法(forward substitution algorithm)求取，从而避免了对矩阵的求逆运算，提高了平方根容积卡尔曼滤波算法的数值稳定性。

第四步，返回第二步，进行迭代滤波。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims

1.基于容积卡尔曼滤波的无线电定位方法，包括如下步骤：

第一步，设置平方根容积卡尔曼滤波的初值；

第二步，完成平方根容积卡尔曼滤波的时间更新；

第三步，完成平方根容积卡尔曼滤波的量测更新，即得到包括接收机的位置、速度状态的信息；

第四步，返回第二步，进行迭代滤波。

2.根据权利要求1所述的基于容积卡尔曼滤波的无线电定位方法，其特征在于：所述第一步，设置平方根容积卡尔曼滤波的初值，包括：接收机的初始位置估计误差方差的平方根初值S(0|0)。

3.根据权利要求1所述的基于容积卡尔曼滤波的无线电定位方法，其特征在于：所述第二步，平方根容积卡尔曼滤波的时间更新包括如下步骤：

步骤2.1：选取容积点，并经过状态方程传播；

<mrow> <mover> <mi>x</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>&omega;</mi> <mi>i</mi> </msub> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>&chi;</mi> <mi>i</mi> <mo>*</mo> </msubsup> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <msup> <mi>&Delta;x</mi> <mo>*</mo> </msup> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <mo>&lsqb;</mo> <msqrt> <msub> <mi>&omega;</mi> <mi>i</mi> </msub> </msqrt> <mrow> <mo>(</mo> <msubsup> <mi>&chi;</mi> <mi>i</mi> <mo>*</mo> </msubsup> <mo>(</mo> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> <mo>-</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mo>(</mo> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mo>&rsqb;</mo> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mn>2</mn> <mi>n</mi> </mrow>

S(k|k-1)＝Tria{[Δx^*(k|k-1)S_Q(k-1)]^T}

4.根据权利要求1所述的基于容积卡尔曼滤波的无线电定位方法，其特征在于：所述第三步，平方根容积卡尔曼滤波的量测更新包括如下步骤：

步骤3.1：计算新的容积点，并将其通过观测方程进行传播；

Z_i(k|k-1)＝h(χ_i(k|k-1))

<mrow> <mover> <mi>z</mi> <mo>^</mo> </mover> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>&omega;</mi> <mi>i</mi> </msub> <munderover> <mo>&Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>2</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>Z</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

<mrow> <mi>&Delta;</mi> <mi>Z</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <mo>&lsqb;</mo> <msqrt> <msub> <mi>&omega;</mi> <mi>i</mi> </msub> </msqrt> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>i</mi> </msub> <mo>(</mo> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> <mo>-</mo> <mover> <mi>z</mi> <mo>^</mo> </mover> <mo>(</mo> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mo>&rsqb;</mo> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mn>2</mn> <mi>n</mi> </mrow>

S_z(k|k-1)＝Tria{[ΔZ(k|k-1)S_R(k)]^T}

S_R(k-1)为k-1次滤波中的量测噪声方差矩阵

步骤3.3：计算协方差矩阵；

<mrow> <mi>&Delta;</mi> <mi>x</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> <mo>=</mo> <mo>&lsqb;</mo> <msqrt> <msub> <mi>&omega;</mi> <mi>i</mi> </msub> </msqrt> <mrow> <mo>(</mo> <msub> <mi>&chi;</mi> <mi>i</mi> </msub> <mo>(</mo> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> <mo>-</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> <mo>(</mo> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mo>&rsqb;</mo> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mo>...</mo> <mo>,</mo> <mn>2</mn> <mi>n</mi> </mrow>

P_xz(k|k-1)＝Δx(k|k-1)gΔZ^T(k|k-1)

步骤3.4：计算卡尔曼增益K(k)，完成状态估计；

<mrow> <mi>S</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>T</mi> <mi>r</mi> <mi>i</mi> <mi>a</mi> <mo>{</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>(</mo> <mi>&Delta;</mi> <mi>x</mi> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> <mo>-</mo> <mi>K</mi> <mo>(</mo> <mi>k</mi> <mo>)</mo> <mi>&Delta;</mi> <mi>z</mi> <mo>(</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> <mo>)</mo> </mrow> </mtd> <mtd> <mrow> <mi>K</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <msqrt> <mrow> <mi>R</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </msqrt> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>}</mo> </mrow>

其中，为对接收机状态的估计值，S(k|k)对应估计误差方差矩阵。z(k)为第k次滤波过程中，接收机通过与基站通信，获取到的其与基站的距离或角度等观测向量。