CN108490433A - 基于序贯滤波的空时偏差联合估计与补偿方法及系统 - Google Patents

基于序贯滤波的空时偏差联合估计与补偿方法及系统 Download PDF

Info

Publication number
CN108490433A
CN108490433A CN201810120311.XA CN201810120311A CN108490433A CN 108490433 A CN108490433 A CN 108490433A CN 201810120311 A CN201810120311 A CN 201810120311A CN 108490433 A CN108490433 A CN 108490433A
Authority
CN
China
Prior art keywords
state
moment
augmented
deviation
matrix
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201810120311.XA
Other languages
English (en)
Other versions
CN108490433B (zh
Inventor
周共健
卜石哲
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201810120311.XA priority Critical patent/CN108490433B/zh
Publication of CN108490433A publication Critical patent/CN108490433A/zh
Application granted granted Critical
Publication of CN108490433B publication Critical patent/CN108490433B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01D—MEASURING NOT SPECIALLY ADAPTED FOR A SPECIFIC VARIABLE; ARRANGEMENTS FOR MEASURING TWO OR MORE VARIABLES NOT COVERED IN A SINGLE OTHER SUBCLASS; TARIFF METERING APPARATUS; MEASURING OR TESTING NOT OTHERWISE PROVIDED FOR
    • G01D21/00—Measuring or testing not otherwise provided for
    • G01D21/02—Measuring two or more variables by means not covered by a single other subclass

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明涉及一种基于序贯滤波的空时偏差联合估计与补偿方法及系统，其中方法包括：利用UKF滤波算法估计目标状态，采用不敏变换计算状态采样点和对应的权值；利用状态采样点和对应的权值计算预测状态采样点，预测状态和预测状态误差协方差矩阵；利用预测状态采样点计算预测量测采样点，以及第k个量测时刻的预测量测向量；利用预测量测采样点和第k个量测时刻的预测量测向量计算新息矩阵以及状态和观测之间的互协方差矩阵，并获得卡尔曼增益；更新扩维状态向量和扩维状态误差协方差矩阵的估计。本发明用空时偏差对目标状态进行扩维，对收到的传感器量测数据进行序贯处理，基于UKF方法对扩维后的状态向量进行估计，实现了目标状态和空时偏差的同时估计。

Description

基于序贯滤波的空时偏差联合估计与补偿方法及系统
技术领域
本发明涉及雷达目标跟踪技术领域，尤其涉及一种基于序贯滤波的空时偏差联合估计与补偿方法及系统。
背景技术
在多传感器信息融合系统中，传感器量测数据可能存在的系统误差和传感器信息的时间不同步会影响信息的正确融合，目前多数算法研究的是时间对齐和空间偏差配准问题。
现有技术中提出了一种利用卡尔曼滤波来估计传感器空间偏差参数的方法，该方法以一个传感器为参考，利用多个传感器对目标位置量测值的微分估算出传感器的空间偏差参数，后把其余各传感器配准到该传感器的参考坐标系中，消除传感器空间偏差，适用于传感器的配准误差和姿态误差较小的情况。现有技术中还提出了另一种精确极大似然法(EML)，该方法利用传感器在系统平面中的量测值，运用极大似然法则对目标的状态和传感器空间偏差同时进行估计，运用了两步递归优化法来加快估计的收敛速度，但由于该方法是基于二维区域性平面中立体投影进行的，存在数据的误报和配准模型不准确的问题。上述方法只对空间偏差配准和补偿问题进行处理，未考虑多传感器融合中的时延问题，仅在传感器的时延和量测时刻准确已知的情况下可实现空间偏差配准。
现有技术中提出了一种内插外推时间对齐方法，以其中一个传感器的量测时刻为参考，将其它传感器的量测数据通过内插和外推的方式求出这些传感器在该参考时刻对应的量测值，其缺点在于要求量测数据的时戳精确可知，实质解决的是数据率不同时传感器的同步问题。另有方法中采用最小二乘规则将高数据率传感器的n次量测数据融合到指定时刻，与低数据率传感器在该时刻的的量测数据融合。上述算法在研究中均假设量测数据的时戳是准确的，而在实际应用中，传感器量测数据的时戳可能存在未知时延，即存在时间偏差问题，这使得多传感器之间的同步问题变得更为复杂。
针对协同驾驶系统中的融合和时间偏差配准问题，现有技术中还提出了一种基于UKF的空时偏差配准方法，同时完成多个异类传感器空时偏差的配准以及目标状态的融合估计，其缺点在于只考虑了传感器数据率相同的情况，在传感器的数据率不同时，该算法所构建的模型将不再适用。针对摄像机-惯性导航系统时间偏差问题，现有技术中提出了一种三维运动估计和在线时间偏差配准模型，将时间偏差加入状态向量中，在线的估计惯性测量单元状态，惯性测量单元和摄像机间的空间外部配准参数和时间偏差，但该方法需首先用惯性测量单元量测数据估计运动状态，利用所得的惯性测量单元状态估计和摄像机量测数据估计空间配准参数和时间偏差，其缺点在于需要两步处理，未能同时估计平台运动状态和传感器间的时间偏差。此外，还存在一种通用方法联合估计摄像机，惯性测量单元和激光测距仪之间的时间偏差和空间转换参数，该方法使用了离线极大似然估计器，在传感器参数发生变化时不能做到实时处理；且该方法需用L-M神经网络算法对大量量测数据在优化过程中进行迭代处理，计算量较大。
由此可见，上述算法在解决时间偏差配准问题时存在局限性。
发明内容
本发明要解决的技术问题在于，针对现有方法在解决时间偏差配准问题时存在局限性的问题，提出一种基于序贯滤波的空时偏差联合估计与补偿方法，在线地实现目标状态和空时偏差的同时估计。
为了解决上述技术问题，本发明第一方面，提供了一种基于序贯滤波的空时偏差联合估计与补偿方法，包括以下步骤：
S1、利用UKF滤波算法估计目标状态，采用不敏变换计算状态采样点和对应的权值；
S2、利用状态采样点和对应的权值计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)；
S3、利用预测状态采样点计算预测量测采样点，以及第k个量测时刻的预测量测向量
S4、利用预测量测采样点和第k个量测时刻的预测量测向量计算新息矩阵S(k)以及状态和观测之间的互协方差矩阵Pxz(k)，并获得卡尔曼增益K(k)＝Pxz(k)S(k)；
S5、更新扩维状态向量和扩维状态误差协方差矩阵的估计：
P(k|k)＝P(k|k-1)-K(k)S(k)K′(k)；
其中为扩维状态向量的估计，为步骤S2中获得的预测状态，K(k)为步骤S4中获得的卡尔曼增益，Z(k)为第k个时刻量测数据，为第k个量测时刻的预测量测向量，P(k|k)为扩维状态误差协方差矩阵的估计，P(k|k-1)为步骤S2获得的预测状态误差协方差矩阵，S(k)为步骤S4获得的新息矩阵。
在根据本发明所述的基于序贯滤波的空时偏差联合估计与补偿方法中，优选地，所述步骤S1中通过以下公式计算：
其中ξj(k-1|k-1)和Gj分别为第k-1量测时刻的状态采样点和权值，和P(k-1|k-1)分别为k-1时刻扩维状态向量A(k-1)及扩维状态误差协方差矩阵P(k-1)的估计，m为扩维状态向量的维数，λ为用于决定第k-1个量测时刻状态估计周围ξ采样点的分布状态的尺度参数，且满足(m+λ)≠0；为的第j行或第j列。
在根据本发明所述的基于序贯滤波的空时偏差联合估计与补偿方法中，优选地，当使用2个传感器进行采样时，步骤S1中使用的扩维状态向量A(k)和扩维状态误差协方差矩阵P(k)分别表示为：
其中A(k)＝[x(k),y(k),为目标在第k个量测时刻的状态向量，(x(k),y(k))为第k个量测时刻目标的位置，为第k个量测时刻目标的速度；Δρ1(k)和为第k个量测时刻传感器1的距离和角度偏差，Δρ2(k)和为第k个量测时刻传感器2的距离和角度偏差，Δt(k)为两传感器的相对时间偏差；Pxx为目标状态误差协方差矩阵，和分别为传感器1和传感器2空间偏差协方差矩阵，Pt为时间偏差协方差矩阵。
在根据本发明所述的基于序贯滤波的空时偏差联合估计与补偿方法中，优选地，所述步骤S2中通过以下公式计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)：
ξj(k|k-1)＝Γ(k)ξj(k-1|k-1)；
其中ξj(k|k-1)为第k个量测时刻的预测状态采样点，ξj(k-1|k-1)为第k-1量测时刻的状态采样点，Γ(k)为状态转移矩阵，Gj为状态采样点的权值；ΔAj(k|k-1)为预测状态误差，Q(k)为过程噪声协方差矩阵。
在根据本发明所述的基于序贯滤波的空时偏差联合估计与补偿方法中，优选地，所述步骤S3中使用通过以下公式计算预测量测采样点ηj(k|k-1)，以及第k个量测时刻的预测量测向量
ηj(k|k-1)＝h(k,ξj(k|k-1))
其中h()为量测矩阵，第k个量测时刻的预测量测向量为第k个量测时刻的量测数据Z(k)的预测；Gj为采样点的权值，m为扩维状态向量的维数。
本发明第二方面，提供了一种基于序贯滤波的空时偏差联合估计与补偿系统，包括：
第一处理单元、用于利用UKF滤波算法估计目标状态，采用不敏变换计算状态采样点和对应的权值；
第二处理单元、用于利用状态采样点和对应的权值计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)；
第三处理单元、用于利用预测状态采样点计算预测量测采样点，以及第k个量测时刻的预测量测向量
第四处理单元、用于利用预测量测采样点和第k个量测时刻的预测量测向量计算新息矩阵S(k)以及状态和观测之间的互协方差矩阵Pxz(k)，并获得卡尔曼增益K(k)＝Pxz(k)S(k)；
第五处理单元、用于更新扩维状态向量和扩维状态误差协方差矩阵的估计：
P(k|k)＝P(k|k-1)-K(k)S(k)K′(k)；
其中为扩维状态向量的估计，为第二处理单元中获得的预测状态，K(k)为第四处理单元中获得的卡尔曼增益，Z(k)为第k个时刻量测数据，为第k个量测时刻的预测量测向量，P(k|k)为扩维状态误差协方差矩阵的估计，P(k|k-1)为第二处理单元中获得的预测状态误差协方差矩阵，S(k)为第四处理单元中获得的新息矩阵。
在根据本发明所述的基于序贯滤波的空时偏差联合估计与补偿系统中，优选地，所述第一处理单元通过以下公式计算：
其中ξj(k-1|k-1)和Gj分别为第k-1量测时刻的状态采样点和权值，和P(k-1|k-1)分别为k-1时刻扩维状态向量A(k-1)及扩维状态误差协方差矩阵P(k-1)的估计，m为扩维状态向量的维数，λ为用于决定第k-1个量测时刻的状态估计周围ξ采样点的分布状态的尺度参数，且满足(m+λ)≠0；为的第j行或第j列。
在根据本发明所述的基于序贯滤波的空时偏差联合估计与补偿系统中，优选地，当使用2个传感器进行采样时，所述第一处理单元使用的扩维状态向量A(k)和扩维状态误差协方差矩阵P(k)分别表示为：
其中A(k)＝[x(k),y(k),为目标在第k个量测时刻的状态向量，(x(k),y(k))为第k个量测时刻目标的位置，为第k个量测时刻目标的速度；Δρ1(k)和为第k个量测时刻传感器1的距离和角度偏差，Δρ2(k)和为第k个量测时刻传感器2的距离和角度偏差，Δt(k)为两传感器的相对时间偏差；Pxx为目标状态误差协方差矩阵，和分别为传感器1和传感器2空间偏差协方差矩阵，Pt为时间偏差协方差矩阵。
在根据本发明所述的基于序贯滤波的空时偏差联合估计与补偿系统中，优选地，所述第二处理单元通过以下公式计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)：
ξj(k|k-1)＝Γ(k)ξj(k-1|k-1)；
其中ξj(k|k-1)为第k个量测时刻的预测状态采样点，ξj(k-1|k-1)为第k-1量测时刻的状态采样点，Γ(k)为状态转移矩阵，Gj为状态采样点的权值；ΔAj(k|k-1)为预测状态误差，Q(k)为过程噪声协方差矩阵。
在根据本发明所述的基于序贯滤波的空时偏差联合估计与补偿系统中，优选地，所述第三处理单元使用通过以下公式计算预测量测采样点ηj(k|k-1)，以及第k个量测时刻的预测量测向量
ηj(k|k-1)＝h(k,ξj(k|k-1))
其中h()为量测矩阵，第k个量测时刻的预测量测向量为第k个量测时刻的量测数据Z(k)的预测；Gj为采样点的权值，m为扩维状态向量的维数。
实施本发明的基于序贯滤波的空时偏差联合估计与补偿方法及系统，具有以下有益效果：本发明用空时偏差对目标状态进行扩维，对收到的传感器量测数据进行序贯处理，基于UKF方法对扩维后的状态向量进行估计，实现了目标状态和空时偏差的同时估计。
附图说明
图1为根据本发明优选实施例的基于序贯滤波的空时偏差联合估计与补偿方法的流程图；
图2为相对时间偏差估计结果图；
图3为距离偏差估计结果图；
图4为角度偏差估计结果图；
图5为目标跟踪航迹局部放大图；
图6为目标跟踪航迹完整图；
图7为相对时间偏差估计均方根误差结果图；
图8为距离偏差估计均方根误差结果图；
图9为角度偏差估计均方根误差结果图；
图10为目标位置估计均方根误差结果图；
图11为目标速度估计均方根误差结果图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
为了同时实现空时偏差的配准和目标状态的融合估计，针对传感器数据率不同且存在空时偏差的情况，本发明分析了传感器观测与目标状态及空时偏差的关系，提出了传感器空时偏差与目标状态联合估计序贯滤波模型。本发明的具体实施例中将以两个传感器的情况进行叙述，两传感器数据率不同。传感器观测数据时戳存在固定时延，空时偏差均为固定值。本领域基础技术人员根据该两个传感器的实施可以类推出多个传感器的数据的方案。
本发明提供的基于序贯滤波的空时偏差联合估计与补偿方法，可以实现k＝1,2,…量测时刻目标状态和空时偏差的同时估计。
请参阅图1，为根据本发明优选实施例的基于序贯滤波的空时偏差联合估计与补偿方法的流程图。如图1所示，该实施例提供的基于序贯滤波的空时偏差联合估计与补偿方法包括以下步骤：
首先，在步骤S1中，利用UKF滤波算法估计目标状态，采用不敏变换计算状态采样点ξ和对应的权值G，具体公式为：
其中ξj(k-1|k-1)和Gj分别为第k-1量测时刻的状态采样点和权值，和P(k-1|k-1)分别为k-1时刻扩维状态向量A(k-1)及扩维状态误差协方差矩阵P(k-1)的估计，m为扩维状态向量的维数，λ为用于决定第k-1个量测时刻的状态估计周围ξ采样点的分布状态的尺度参数，且满足(m+λ)≠0；为的第j行或第j列。
当使用2个传感器进行采样时，该步骤S1中使用的第k个量测时刻的扩维状态向量A(k)和扩维状态误差协方差矩阵P(k)分别表为：
其中为目标在第k个量测时刻的状态向量，(x(k),y(k))为第k个量测时刻目标的位置，为第k个量测时刻目标的速度；Δρ1(k)和为第k个量测时刻传感器1的距离和角度偏差，Δρ2(k)和为第k个量测时刻传感器2的距离和角度偏差，Δt(k)为两传感器的相对时间偏差；Pxx为目标状态误差协方差矩阵，和分别为传感器1和传感器2空间偏差协方差矩阵，Pt为时间偏差协方差矩阵。若传感器数目s＞3时，需要取其中一个传感器作为参考传感器，计算其他传感器与该传感器的相对时间偏差。不失一般性，将以传感器1为参考传感器。此时所得扩维状态向量和可分别表示为
其中Δρs(k)和分别为第k个量测时刻传感器s的的距离和角度偏差，为传感器s相对于传感器1的时间偏差，为传感器s的空间偏差协方差矩阵，为传感器s的相对于传感器1的时间偏差协方差矩阵。
随后，在步骤S2中，利用状态采样点和对应的权值计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)，具体公式为：
ξj(k|k-1)＝Γ(k)ξj(k-1|k-1)；
其中ξj(k|k-1)为第k个量测时刻的预测状态采样点，ξj(k-1|k-1)为第k-1量测时刻的状态采样点，Γ(k)为状态转移矩阵，Gj为采样点的权值；ΔAj(k|k-1)为预测状态误差，Q(k)为过程噪声协方差矩阵。
状态转移矩阵Γ(k)的具体公式为：
其中I2表示2维单位矩阵，Γxx(k)表示对应于目标状态部分的状态转移矩阵，具体公式为：
式中ΔT表示前后两个状态之间对应的时差，给定其初始值为0，即假设两传感器同时得到第一个量测数据；
随后，在步骤S3中，利用预测状态采样点计算预测量测采样点，以及第k个量测时刻的预测量测向量具体公式为：
ηj(k|k-1)＝h(k,ξj(k|k-1))；
其中h()为量测矩阵，ηj(k|k-1)为预测量测采样点，第k个量测时刻的预测量测向量为第k个量测时刻的量测数据Z(k)的预测；Gj为状态采样点的权值，m为扩维状态向量的维数。
当使用2个传感器进行采样时，步骤S3中使用的第k个量测时刻的量测数据Z(k)根据第k个量测时刻和第k-1个量测时刻所得量测数据来源的不同得到四种状态转移矩阵和量测方程的组合。具体为：
1)第k个量测时刻所得数据是传感器2的第n个量测数据，第k-1个量测时刻所得数据是传感器2第n-1个量测数据，此时对应于目标状态部分的状态转移矩阵为：
其中ΔT＝T2，T2为第2个传感器的量测周期，对应的量测方程为：
其中w(k)为第k个量测时刻的量测噪声；
2)第k个量测时刻所得数据是传感器2的第n个量测数据，第k-1时刻所得数据是传感器1第m个量测数据，此时对应于目标状态部分的状态转移矩阵为：
其中ΔT＝(n-1)*T2-(m-1)*T1，T1为传感器1的量测周期，对应的量测方程为：
3)第k个量测时刻所得数据是传感器1的第m个量测数据，第k-1时刻所得数据是传感器2第n个量测数据，此时对应于目标状态部分的状态转移矩阵为：
其中ΔT＝(m-1)*T1-(n-1)*T2，对应的量测方程为：
4)第k个量测时刻所得数据是传感器1的第m个量测数据，第k-1时刻所得数据是传感器1第m-1个量测数据，此时对应于目标状态部分的状态转移矩阵为：
其中ΔT＝T1，对应的量测方程为：
随后，在步骤S4中，利用预测量测采样点和第k个量测时刻的预测量测向量计算新息矩阵S(k)以及状态和观测之间的互协方差矩阵Pxz(k)，并获得卡尔曼增益K(k)＝Pxz(k)S(k)；
具体公式为：
式中R(k)为量测噪声协方差矩阵，ΔZj(k|k-1)为预测量测误差，ηj(k|k-1)为预测量测采样点，为第k个量测时刻的预测量测向量，ΔAj(k|k-1)为预测状态误差；
利用上述信息可得卡尔曼增益K(k)：
K(k)＝Pxz(k)S(k)；
最后，在步骤S5中，更新扩维状态向量和扩维状态误差协方差矩阵的估计：具体公式为：
P(k|k)＝P(k|k-1)-K(k)S(k)K′(k)；
其中为扩维状态向量的估计，为步骤S2中获得的预测状态，K(k)为步骤S4中获得的卡尔曼增益，Z(k)为第k个量测时刻量测数据，为第k个量测时刻的预测量测向量，P(k|k)为扩维状态误差协方差矩阵的估计，P(k|k-1)为步骤S2获得的预测状态误差协方差矩阵，S(k)为步骤S4获得的新息矩阵。
因此为扩维状态向量的估计，包含目标状态和传感器空间偏差和相对时间偏差，从而同时实现目标状态和空时偏差的估计。
本发明还提供了一种基于序贯滤波的空时偏差联合估计与补偿系统。该实施例提供的系统包括：
第一处理单元，用于利用UKF滤波算法估计目标状态，采用不敏变换计算状态采样点和对应的权值。该第一处理单元的计算过程与上述方法中步骤S1一致，在此不再赘述。
第二处理单元，用于利用状态采样点和对应的权值计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)。该第二处理单元的计算过程与上述方法中步骤S2一致，在此不再赘述。
第三处理单元，用于利用预测状态采样点计算预测量测采样点，以及第k个量测时刻的预测量测向量该第三处理单元的计算过程与上述方法中步骤S3一致，在此不再赘述。
第四处理单元，用于利用预测量测采样点和第k个量测时刻的预测量测向量计算新息矩阵S(k)以及状态和观测之间的互协方差矩阵Pxz(k)，并获得卡尔曼增益K(k)＝Pxz(k)S(k)。该第四处理单元的计算过程与上述方法中步骤S4一致，在此不再赘述。
第五处理单元，用于更新扩维状态向量和扩维状态误差协方差矩阵的估计：
P(k|k)＝P(k|k-1)-K(k)S(k)K′(k)；
其中为扩维状态向量的估计，为第二处理单元中获得的预测状态，K(k)为第四处理单元中获得的卡尔曼增益，Z(k)为第k个时刻量测数据，为第k个量测时刻的预测量测向量，P(k|k)为扩维状态误差协方差矩阵的估计，P(k|k-1)为第二处理单元中获得的预测状态误差协方差矩阵，S(k)为第四处理单元中获得的新息矩阵。该第五处理单元的计算过程与上述方法中步骤S5一致。
下面通过仿真对本发明的效果进行验证。
首先给出单次仿真中各时刻空时偏差估计以及目标跟踪航迹，仿真结果如图2至图6所示。其中图2为相对时间偏差估计结果图，图3为距离偏差估计结果图，图4为角度偏差估计结果图，图5为目标跟踪航迹局部放大图，图6为目标跟踪航迹完整图。
由图2-图4可看出，随着滤波过程的进行本发明采用的基于序贯处理的空时偏差联合估计与补偿方法(SP-ST-BR)所得时间偏差和空间偏差估计逐渐向偏差真实值附近收敛，说明本发明方法有较好的滤波性能。由图3和图4可看出，常规方法(R-BR)在处理过程中未考虑观测数据中时间偏差的存在，所得距离和角度偏差估计的性能较差，说明了本发明所使用方法考虑时间偏差的必要性。由图5和图6可看出，含有空时偏差的量测航迹，经过本发明的处理之后，所得滤波航迹与真实航迹之间的偏移量大大减小，说明本发明能较好的估计目标状态。
下面给出100次Monte Carlo仿真中空时偏差估计和目标状态估计均方根误差的仿真结果，如图7～图11所示。其中，图7为相对时间偏差估计均方根误差结果图；图8为距离偏差估计均方根误差结果图；图9为角度偏差估计均方根误差结果图；图10为目标位置估计均方根误差结果图；图11为目标速度估计均方根误差结果图。由图7-图11可看出，利用本发明所使用的基于序贯处理的空时偏差联合估计与补偿方法(SP-ST-BR)所得目标状态估计和空时偏差估计的均方根误差均呈现出收敛的趋势，随着滤波过程的进行能达到稳定滤波状态,能实现空时偏差和目标状态的有效估计；由图8-图11可看出，常规方法(R-BR)在处理过程中未考虑观测数据中时间偏差的存在，所得目标状态估计和空间偏差估计的均方根误差性能较差，说明了本发明所使用方法考虑时间偏差的必要性。
应该理解地是，本发明中基于序贯滤波的空时偏差联合估计与补偿方法及系统的原理相同，因此对基于序贯滤波的空时偏差联合估计与补偿方法的实施例的详细阐述也适用于基于序贯滤波的空时偏差联合估计与补偿系统。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种基于序贯滤波的空时偏差联合估计与补偿方法，其特征在于，包括以下步骤：
S1、利用UKF滤波算法估计目标状态，采用不敏变换计算状态采样点和对应的权值；
S2、利用状态采样点和对应的权值计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)；
S3、利用预测状态采样点计算预测量测采样点，以及第k个量测时刻的预测量测向量
S4、利用预测量测采样点和第k个量测时刻的预测量测向量计算新息矩阵S(k)以及状态和观测之间的互协方差矩阵Pxz(k)，并获得卡尔曼增益K(k)＝Pxz(k)S(k)；
S5、更新扩维状态向量和扩维状态误差协方差矩阵的估计：
P(k|k)＝P(k|k-1)-K(k)S(k)K′(k)；
其中为扩维状态向量的估计，为步骤S2中获得的预测状态，K(k)为步骤S4中获得的卡尔曼增益，Z(k)为第k个量测时刻量测数据，为第k个量测时刻的预测量测向量，P(k|k)为扩维状态误差协方差矩阵的估计，P(k|k-1)为步骤S2获得的预测状态误差协方差矩阵，S(k)为步骤S4获得的新息矩阵。
2.根据权利要求1所述的基于序贯滤波的空时偏差联合估计与补偿方法，其特征在于，所述步骤S1中通过以下公式计算：
其中ξj(k-1|k-1)和Gj分别为第k-1量测时刻的状态采样点和权值，和P(k-1|k-1)分别为k-1时刻扩维状态向量A(k-1)及扩维状态误差协方差矩阵P(k-1)的估计，m为扩维状态向量的维数，λ为用于决定第k-1个量测时刻状态估计周围ξ采样点的分布状态的尺度参数，且满足(m+λ)≠0；为的第j行或第j列。
3.根据权利要求2所述的基于序贯滤波的空时偏差联合估计与补偿方法，其特征在于，当使用2个传感器进行采样时，步骤S1中使用的扩维状态向量A(k)和扩维状态误差协方差矩阵P(k)分别表示为：
其中为目标在第k个量测时刻的状态向量，(x(k),y(k))为第k个量测时刻目标的位置，为第k个量测时刻目标的速度；Δρ1(k)和为第k个量测时刻传感器1的距离和角度偏差，Δρ2(k)和为第k个量测时刻传感器2的距离和角度偏差，Δt(k)为两传感器的相对时间偏差；Pxx为目标状态误差协方差矩阵，和分别为传感器1和传感器2空间偏差协方差矩阵，Pt为时间偏差协方差矩阵。
4.根据权利要求1所述的基于序贯滤波的空时偏差联合估计与补偿方法，其特征在于，所述步骤S2中通过以下公式计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)：
ξj(k|k-1)＝Γ(k)ξj(k-1|k-1)；
其中ξj(k|k-1)为第k个量测时刻的预测状态采样点，ξj(k-1|k-1)为第k-1量测时刻的状态采样点，Γ(k)为状态转移矩阵，Gj为状态采样点的权值；ΔAj(k|k-1)为预测状态误差，Q(k)为过程噪声协方差矩阵。
5.根据权利要求1所述的基于序贯滤波的空时偏差联合估计与补偿方法，其特征在于，所述步骤S3中使用通过以下公式计算预测量测采样点ηj(k|k-1)，以及第k个量测时刻的预测量测向量
ηj(k|k-1)＝h(k,ξj(k|k-1))
其中h()为量测矩阵，第k个量测时刻的预测量测向量为第k个量测时刻的量测数据Z(k)的预测；Gj为采样点的权值，m为扩维状态向量的维数。
6.一种基于序贯滤波的空时偏差联合估计与补偿系统，其特征在于，包括：
第一处理单元、用于利用UKF滤波算法估计目标状态，采用不敏变换计算状态采样点和对应的权值；
第二处理单元、用于利用状态采样点和对应的权值计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)；
第三处理单元、用于利用预测状态采样点计算预测量测采样点，以及第k个量测时刻的预测量测向量
第四处理单元、用于利用预测量测采样点和第k个量测时刻的预测量测向量计算新息矩阵S(k)以及状态和观测之间的互协方差矩阵Pxz(k)，并获得卡尔曼增益K(k)＝Pxz(k)S(k)；
第五处理单元、用于更新扩维状态向量和扩维状态误差协方差矩阵的估计：
P(k|k)＝P(k|k-1)-K(k)S(k)K′(k)；
其中为扩维状态向量的估计，为第二处理单元中获得的预测状态，K(k)为第四处理单元中获得的卡尔曼增益，Z(k)为第k个时刻量测数据，为第k个量测时刻的预测量测向量，P(k|k)为扩维状态误差协方差矩阵的估计，P(k|k-1)为第二处理单元中获得的预测状态误差协方差矩阵，S(k)为第四处理单元中获得的新息矩阵。
7.根据权利要求6所述的基于序贯滤波的空时偏差联合估计与补偿系统，其特征在于，所述第一处理单元通过以下公式计算：
其中ξj(k-1|k-1)和Gj分别为第k-1量测时刻的状态采样点和权值，和P(k-1|k-1)分别为k-1时刻扩维状态向量A(k-1)及扩维状态误差协方差矩阵P(k-1)的估计，m为扩维状态向量的维数，λ为用于决定第k-1个量测时刻状态估计周围ξ采样点的分布状态的尺度参数，且满足(m+λ)≠0；为的第j行或第j列。
8.根据权利要求7所述的基于序贯滤波的空时偏差联合估计与补偿系统，其特征在于，当使用2个传感器进行采样时，所述第一处理单元使用的扩维状态向量A(k)和扩维状态误差协方差矩阵P(k)分别表示为：
其中为目标在第k个量测时刻的状态向量，(x(k),y(k))为第k个量测时刻目标的位置，为第k个量测时刻目标的速度；Δρ1(k)和为第k个量测时刻传感器1的距离和角度偏差，Δρ2(k)和为第k个量测时刻传感器2的距离和角度偏差，Δt(k)为两传感器的相对时间偏差；Pxx为目标状态误差协方差矩阵，和分别为传感器1和传感器2空间偏差协方差矩阵，Pt为时间偏差协方差矩阵。
9.根据权利要求6所述的基于序贯滤波的空时偏差联合估计与补偿系统，其特征在于，所述第二处理单元通过以下公式计算预测状态采样点，预测状态和预测状态误差协方差矩阵P(k|k-1)：
ξj(k|k-1)＝Γ(k)ξj(k-1|k-1)；
其中ξj(k|k-1)为第k个量测时刻的预测状态采样点，ξj(k-1|k-1)为第k-1量测时刻的状态采样点，Γ(k)为状态转移矩阵，Gj为状态采样点的权值；ΔAj(k|k-1)为预测状态误差，Q(k)为过程噪声协方差矩阵。
10.根据权利要求6所述的基于序贯滤波的空时偏差联合估计与补偿方法，其特征在于，所述第三处理单元使用通过以下公式计算预测量测采样点ηj(k|k-1)，以及第k个量测时刻的预测量测向量
ηj(k|k-1)＝h(k,ξj(k|k-1))
其中h()为量测矩阵，第k个量测时刻的预测量测向量为第k个量测时刻的量测数据Z(k)的预测；Gj为采样点的权值，m为扩维状态向量的维数。
CN201810120311.XA 2018-02-07 2018-02-07 基于序贯滤波的空时偏差联合估计与补偿方法及系统 Active CN108490433B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810120311.XA CN108490433B (zh) 2018-02-07 2018-02-07 基于序贯滤波的空时偏差联合估计与补偿方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810120311.XA CN108490433B (zh) 2018-02-07 2018-02-07 基于序贯滤波的空时偏差联合估计与补偿方法及系统

Publications (2)

Publication Number Publication Date
CN108490433A true CN108490433A (zh) 2018-09-04
CN108490433B CN108490433B (zh) 2019-05-21

Family

ID=63344617

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810120311.XA Active CN108490433B (zh) 2018-02-07 2018-02-07 基于序贯滤波的空时偏差联合估计与补偿方法及系统

Country Status (1)

Country Link
CN (1) CN108490433B (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109800819A (zh) * 2019-01-28 2019-05-24 哈尔滨工业大学 周期任意的多传感器空时偏差补偿方法和装置
CN110108984A (zh) * 2019-05-24 2019-08-09 中国南方电网有限责任公司超高压输电公司检修试验中心 电力巡线激光雷达系统多传感器的空间关系同步方法
CN110455309A (zh) * 2019-08-27 2019-11-15 清华大学 具备在线时间校准的基于msckf的视觉惯性里程计
CN110720096A (zh) * 2019-07-03 2020-01-21 深圳市速腾聚创科技有限公司 一种多传感器状态估计方法、装置及终端设备
CN110807942A (zh) * 2019-09-24 2020-02-18 联创汽车电子有限公司 智能驾驶汽车航迹更新方法及其更新系统
CN111426322A (zh) * 2020-04-09 2020-07-17 北京航空航天大学 同时估计状态和输入的自适应目标跟踪滤波方法及系统
CN111679151A (zh) * 2020-05-27 2020-09-18 武汉华电顺承科技有限公司 一种用于录波数据双端测距的同步对齐点向量计算方法
CN112083400A (zh) * 2020-08-21 2020-12-15 达闼机器人有限公司 运动物体及其传感器的标定方法、装置、存储介质
CN112285697A (zh) * 2020-10-20 2021-01-29 哈尔滨工业大学 一种多传感器多目标空时偏差校准与融合方法
CN112747753A (zh) * 2019-10-29 2021-05-04 北京初速度科技有限公司 一种车辆状态确定方法及车载终端中的处理器
CN113804194A (zh) * 2021-10-25 2021-12-17 中国第一汽车股份有限公司 一种行驶设备的定位方法、装置、设备及存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101615794A (zh) * 2009-08-05 2009-12-30 河海大学 基于无迹变换卡尔曼滤波的电力系统动态状态估计方法
CN101819041A (zh) * 2010-04-16 2010-09-01 北京航空航天大学 自进化anfis与ukf结合的gps/mems-ins组合定位误差动态预测方法
CN102540216A (zh) * 2011-11-14 2012-07-04 北京航空航天大学 一种自适应跟踪环路及实现方法
CN106597498A (zh) * 2017-01-18 2017-04-26 哈尔滨工业大学 多传感器融合系统空时偏差联合校准方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101615794A (zh) * 2009-08-05 2009-12-30 河海大学 基于无迹变换卡尔曼滤波的电力系统动态状态估计方法
CN101819041A (zh) * 2010-04-16 2010-09-01 北京航空航天大学 自进化anfis与ukf结合的gps/mems-ins组合定位误差动态预测方法
CN102540216A (zh) * 2011-11-14 2012-07-04 北京航空航天大学 一种自适应跟踪环路及实现方法
CN106597498A (zh) * 2017-01-18 2017-04-26 哈尔滨工业大学 多传感器融合系统空时偏差联合校准方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
谢青青: "多传感器信息融合空时误差配准技术", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109800819A (zh) * 2019-01-28 2019-05-24 哈尔滨工业大学 周期任意的多传感器空时偏差补偿方法和装置
CN110108984B (zh) * 2019-05-24 2021-07-16 中国南方电网有限责任公司超高压输电公司检修试验中心 电力巡线激光雷达系统多传感器的空间关系同步方法
CN110108984A (zh) * 2019-05-24 2019-08-09 中国南方电网有限责任公司超高压输电公司检修试验中心 电力巡线激光雷达系统多传感器的空间关系同步方法
CN110720096A (zh) * 2019-07-03 2020-01-21 深圳市速腾聚创科技有限公司 一种多传感器状态估计方法、装置及终端设备
CN110720096B (zh) * 2019-07-03 2022-07-08 深圳市速腾聚创科技有限公司 一种多传感器状态估计方法、装置及终端设备
CN110455309A (zh) * 2019-08-27 2019-11-15 清华大学 具备在线时间校准的基于msckf的视觉惯性里程计
CN110807942B (zh) * 2019-09-24 2021-11-02 上海汽车工业(集团)总公司 智能驾驶汽车航迹更新方法及其更新系统
CN110807942A (zh) * 2019-09-24 2020-02-18 联创汽车电子有限公司 智能驾驶汽车航迹更新方法及其更新系统
CN112747753A (zh) * 2019-10-29 2021-05-04 北京初速度科技有限公司 一种车辆状态确定方法及车载终端中的处理器
CN111426322B (zh) * 2020-04-09 2020-12-18 北京航空航天大学 同时估计状态和输入的自适应目标跟踪滤波方法及系统
CN111426322A (zh) * 2020-04-09 2020-07-17 北京航空航天大学 同时估计状态和输入的自适应目标跟踪滤波方法及系统
CN111679151A (zh) * 2020-05-27 2020-09-18 武汉华电顺承科技有限公司 一种用于录波数据双端测距的同步对齐点向量计算方法
CN112083400A (zh) * 2020-08-21 2020-12-15 达闼机器人有限公司 运动物体及其传感器的标定方法、装置、存储介质
CN112285697A (zh) * 2020-10-20 2021-01-29 哈尔滨工业大学 一种多传感器多目标空时偏差校准与融合方法
CN112285697B (zh) * 2020-10-20 2023-09-26 哈尔滨工业大学 一种多传感器多目标空时偏差校准与融合方法
CN113804194A (zh) * 2021-10-25 2021-12-17 中国第一汽车股份有限公司 一种行驶设备的定位方法、装置、设备及存储介质
CN113804194B (zh) * 2021-10-25 2024-03-15 中国第一汽车股份有限公司 一种行驶设备的定位方法、装置、设备及存储介质

Also Published As

Publication number Publication date
CN108490433B (zh) 2019-05-21

Similar Documents

Publication Publication Date Title
CN108490433B (zh) 基于序贯滤波的空时偏差联合估计与补偿方法及系统
CN110375730B (zh) 基于imu和uwb融合的室内定位导航系统
CN104809326B (zh) 一种异步传感器空间配准算法
CN110030994B (zh) 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN107084714B (zh) 一种基于RoboCup3D的多机器人协作目标定位方法
CN109459019A (zh) 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法
CN110823214B (zh) 一种空间完全非合作目标相对位姿和惯量估计方法
WO2021082790A1 (zh) 一种基于IMU的uwb定位异常值处理方法
CN106525003B (zh) 一种基于双目视觉的姿态测量方法
CN108896047B (zh) 分布式传感器网络协同融合与传感器位置修正方法
CN104713560B (zh) 基于期望最大化的多源测距传感器空间配准方法
CN110986968B (zh) 三维重建中实时全局优化和错误回环判断的方法及装置
CN106482736A (zh) 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法
CN109543703A (zh) 传感器数据处理的方法及装置
CN106197428A (zh) 一种利用测量信息优化分布式ekf估计过程的slam方法
CN108267731B (zh) 无人机目标跟踪系统的构建方法及应用
CN103776453A (zh) 一种多模型水下航行器组合导航滤波方法
CN107621266B (zh) 基于特征点跟踪的空间非合作目标相对导航方法
CN109655059B (zh) 一种基于θ-增量学习的视觉-惯性融合导航系统及方法
CN104182609B (zh) 基于去相关的无偏转换量测的三维目标跟踪方法
CN103878770A (zh) 基于速度估计的空间机器人视觉时延误差补偿方法
CN105652250A (zh) 一种基于双层期望最大化的机动目标跟踪技术
CN108731700A (zh) 一种视觉惯性里程计中的加权欧拉预积分方法
CN109764876A (zh) 无人平台的多模态融合定位方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant