CN108446256A - 局部估计自适应两阶段平方根容积滤波的方法 - Google Patents

局部估计自适应两阶段平方根容积滤波的方法 Download PDF

Info

Publication number
CN108446256A
CN108446256A CN201810189568.0A CN201810189568A CN108446256A CN 108446256 A CN108446256 A CN 108446256A CN 201810189568 A CN201810189568 A CN 201810189568A CN 108446256 A CN108446256 A CN 108446256A
Authority
CN
China
Prior art keywords
noise
estimated
moment
square root
estimation
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.)
Pending
Application number
CN201810189568.0A
Other languages
English (en)
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.)
Quzhou University
Original Assignee
Quzhou University
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 Quzhou University filed Critical Quzhou University
Priority to CN201810189568.0A priority Critical patent/CN108446256A/zh
Publication of CN108446256A publication Critical patent/CN108446256A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Operations Research (AREA)
  • Probability & Statistics with Applications (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Algebra (AREA)
  • Evolutionary Biology (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Measurement Of Mechanical Vibrations Or Ultrasonic Waves (AREA)

Abstract

本发明公开了一种局部估计自适应两阶段平方根容积滤波的方法，包括：首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积滤波算法；然后提出自适应两阶段平方根容积滤波算法；在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环。本发明通过在局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF‑G)中，对于量测噪声统计特性的估计是采用Sage‑Husa滤波器直接进行估计，然后将估计出来的统计特性值作为已知条件。

Description

局部估计自适应两阶段平方根容积滤波的方法
技术领域
本发明涉及滤波估计领域，具体地说，特别涉及一种局部估计自适应两阶段平方根容积滤波的方法。
背景技术
非线性滤波算法是在非线性系统模型下利用离散的传感器观测量来估计目标的连续状态并滤去随机噪声的过程。目前几种常见的非线性Kalman滤波有各自的优缺点。扩展Kalman滤波(EKF)将非线性函数进行泰勒展开，忽略高维项进行线性化，但这种方法只适合系统模型足够平滑的弱非线性函数，若系统是强非线性系统，则会因为滤波误差较大而失去有效性，同时在计算时需要计算雅克比矩阵，计算量较大。无迹Kalman滤波(UKF)不需要计算Jacobian矩阵，计算量与EKF相当，在强非线性系统下也有较高的滤波精度。但是UKF没有经过严格的数学推导，在状态维数较高时，滤波精度会下降。采用三度 Spherical-Radial容积规则近似非线性函数传递的后验均值和协方差的容积 Kalman滤波，经过了严密的数学推导，在理论上具有严格的保证。在滤波迭代过程中，有些因素的存在会导致误差协方差矩阵负定，比如数值计算中的舍入误差、初始值误差较大和观测噪声较大等，从而导致滤波器不稳定，甚至无法工作。
发明内容
为了解决现有技术的问题，本发明实施例提供了一种局部估计自适应两阶段平方根容积滤波的方法。所述技术方案如下：
一方面，提供了一种局部估计自适应两阶段平方根容积滤波的方法，包括：
首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积Kalman滤波算法；
然后基于Sage-Husa滤波算法，提出自适应两阶段平方根容积Kalman滤波算法；
在两阶段容积Kalman滤波的过程中，整体的对噪声未知统计特性进行估计，将估计出的噪声统计特性作为已知条件进行递推估计。
进一步的，所述平方根两阶段容积Kalman滤波算法中的系统模型具体为；
带有随机偏差的非线性高斯系统模型可表示为：
其中k是离散时间序列，xk∈Rn×1是系统的状态向量，bk∈Rp×1是系统偏差向量，zk∈Rm×1是量测向量，fk(·)和hk(·)为已知的非线性状态转移函数和量测函数且在xk处连续可微，是系统噪声序列、是偏差噪声序列，υk是量测噪声序列，均为高斯白噪声序列。其中均值为E(υk)＝r，方差和Rk满足如下条件：
令
Hk(Xk)＝hk(xk)+Fkbk
系统模型可以改写为如下形式：
Xk+1＝Fk(Xk)+ωk
Zk＝Hk(Xk)+υk
其统计特性满足如下公式(5-2)所示：
初始状态x0、b0与ωk、υk无关，且满足
进一步的，所述平方根两阶段容积Kalman滤波算法步骤如下：
Step1时间更新:
⑴计算容积点Xi,k-|k-1和通过状态转移函数计算得到的传播容积点
⑵估计k时刻的状态预测值和
⑶估计k时刻的状态误差协方差平方根因子
其中 定义如公式(5-4)所示：
⑷借助Uk估计k时刻的状态误差协方差和其中
Step2量测更新:
⑴计算容积点Xi,k|k-1和通过观测函数计算得到的传播容积点Zi,k|k-1；
⑵估计k时刻的量测预测值
⑶估计k时刻量测误差协方差的平方根因子
Szz,k|k-1＝Tria([∈k|k-1 SR，k]) (5-5)
其中∈k|k-1定义如公式(5-4)所示：
⑷估计k时刻的量测误差协方差
⑸估计k时刻的交叉协方差
⑹估计卡尔曼增益和其中
⑺计算k时刻状态估计值和
⑻计算k时刻状态误差协方差估计值和Vk。
进一步的，所述系统模型如公式(5-1)所示，具有如公式(5-2)所示的噪声统计特性，在噪声统计特性未知或不准确的情况下，基于Sage-Husa算法可得次优常值噪声估计器：
其中是j-1时刻的状态估计值经过非线性状态函数f(·)传播后得到的状态预测值，是j时刻的状态估计值经过非线性量测函数h(·)传播后得到的量测预测值，在容积Kalman滤波算法中有
Xi,j-1|j-1与Xi,j|j-1分别表示j-1与j时刻状态估计值与计算得到的容积点。采用MAP估计器，得到噪声统计特性未知的无偏估计器，结合容积Kalman滤波算法，可得无偏常值噪声估计器：
其中表示新息向量。
进一步的，所述系统模型如公式(5-1)所示，公式(5-2)为其噪声统计特性，如果其噪声为时变的情况，需要减弱或消除陈旧量测信息，加大最近量测信息的作用，因此对求和式中的每一项引入不同的加权系数；采用渐消记忆指数加权的方式对时变噪声进行估计，可得无偏时变噪声估计器：
其中dk-1＝(1-b)/(1-bk)，b为遗忘因子，用来限制滤波器的记忆长度，通常b的取值范围为0.95＜b＜0.99，b的值越大越加重最近量测信息的作用，当噪声变化快时，b的取值应偏大，反之则偏小。
进一步的，所述系统模型形如公式(5-1)所示，在实际应用中，量测噪声容易受到量测误差和环境扰动等方面的影响，因此在公式(5-2)所示的噪声统计特性中，系统噪声ωk为均值为qk，方差为Qk的高斯白噪声序列，量测噪声υk也为高斯白噪声序列，但是统计特性未知；
结合Sage-Husa算法，首先采用局部估计的方式，对未知的量测噪声统计特性进行局部估计，然后再根据两阶段方法，对滤波器进行分解；
进一步的，所述在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环具体为：
局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF-L)中均以上角标L标明以示区分，算法步骤如下：
初始化：
Step1时间更新：
⑴计算容积点Xi,k-|k-1和通过状态转移函数计算得到的传播容积点
⑵由于系统噪声均值不为0，故有
其中
⑶估计k时刻的状态预测值和
得到
⑷估计k时刻的状态误差协方差平方根因子
其中 定义如公式(5-25)所示：
⑸估计k时刻的状态误差协方差和
其中
⑹借助Uk计算和其中
Step2量测更新：
⑴计算容积点Xi，k|k-1和通过观测函数计算得到的传播容积点
⑵由于测量噪声的均值也不为0，估计k时刻的量测预测值
其中
⑶估计k时刻的量测误差协方差的平方根因子
其中
⑷估计k时刻的量测误差协方差
⑸估计k时刻的交叉协方差
⑹估计卡尔曼增益和其中
⑺计算k时刻状态估计值和
⑻计算k时刻状态误差协方差估计值和Vk；
Step3噪声估计：
⑴分别估计k时刻状态噪声的均值和
由公式(5-18)可得
即
得到
⑵分别估计k时刻状态噪声的方差
由公式(5-19)可得
其中即
得到
Step4：如果k<N，N为跟踪时间，转到Step1。
进一步的，在局部估计自适应两阶段平方根容积Kalman滤波算法中，计算系统噪声统计特性时，通过得到系统噪声统计特性，并在两个阶段滤波中各自的表示方法，然后采用Sage-Husa滤波器分别进行估计，将估计出来的统计特性值加入到下一个时刻的递推估计中。
本发明实施例提供的技术方案带来的有益效果是：
本发明通过采用Saga-Husa估计器实时估计噪声的未知统计特性，再将实时更新的统计特性带入Kalman滤波算法，改善噪声统计特性未知系统中的滤波估计效果，再用新得到的系统状态和误差协方差再次修正噪声统计特征，以此形成递推循环；在局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF-G) 中，对于量测噪声统计特性的估计是采用Sage-Husa滤波器直接进行估计，然后将估计出来的统计特性值作为已知条件，从而提高了滤波的跟踪精度，使噪声的估计值逐步趋向于真实值，具有自适应的能力。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例的一种局部估计自适应两阶段平方根容积滤波的方法流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。
本发明提供了一种局部估计自适应两阶段平方根容积滤波的方法，参见图1，包括：
S100：首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积Kalman滤波算法；
S200：然后基于Sage-Husa滤波算法，提出自适应两阶段平方根容积Kalman 滤波算法；
S300：在两阶段容积Kalman滤波的过程中，整体的对噪声未知统计特性进行估计，将估计出的噪声统计特性作为已知条件进行递推估计。
进一步的，所述平方根两阶段容积Kalman滤波算法中的系统模型具体为；
带有随机偏差的非线性高斯系统模型可表示为：
其中k是离散时间序列，xk∈Rn×1是系统的状态向量，bk∈Rp×1是系统偏差向量，zk∈Rm×1是量测向量，fk(·)和hk(·)为已知的非线性状态转移函数和量测函数且在xk处连续可微，是系统噪声序列、是偏差噪声序列，υk是量测噪声序列，均为高斯白噪声序列。其中均值为E(υk)＝r，方差和Rk满足如下条件：
令
Hk(Xk)＝hk(xk)+Fkbk
系统模型可以改写为如下形式：
Xk+1＝Fk(Xk)+ωk
Zk＝Hk(Xk)+υk
其统计特性满足如下公式(5-2)所示：
初始状态x0、b0与ωk、υk无关，且满足
进一步的，具体地，提出容积Kalman滤波的同时，还提出了容积Kalman 滤波的平方根形式，即在递推运算中使用误差协方差阵的平方根，而不用协方差矩阵，这种方法可以提高滤波器的稳定性，抑制滤波发散。在此引入平方根的思想，得到更为稳定的平方根两阶段容积Kalman滤波算法步骤如下：
Step1时间更新:
⑴计算容积点Xi,k-|k-1和通过状态转移函数计算得到的传播容积点
⑵估计k时刻的状态预测值和
⑶估计k时刻的状态误差协方差平方根因子
其中 定义如公式(5-4)所示：
⑷借助Uk估计k时刻的状态误差协方差和其中
Step2量测更新:
⑴计算容积点Xi,k|k-1和通过观测函数计算得到的传播容积点Zi,k|k-1；
⑵估计k时刻的量测预测值
⑶估计k时刻量测误差协方差的平方根因子
Szz,k|k-1＝Tria([∈k|k-1 SR，k]) (5-5)
其中∈k|k-1定义如公式(5-4)所示：
⑷估计k时刻的量测误差协方差
⑸估计k时刻的交叉协方差
⑹估计卡尔曼增益和其中
⑺计算k时刻状态估计值和
⑻计算k时刻状态误差协方差估计值和Vk。
进一步的，具体地，上述的各类容积Kalman滤波算法的推导，前提条件均是假设系统噪声和量测噪声统计特性精确已知，但在实际应用中，系统噪声和量测噪声的统计特性未知或时变的情况普遍存在，这使得常规容积Kalman滤波算法失去了最优性，估计精度下降，甚至引起了滤波发散。本小节主要研究能够对噪声的未知统计特性在线估计修正的自适应两阶段容积Kalman滤波算法。
其中，Sage-Husa算法是无偏次优极大后验噪声估计算法，其原理清晰，计算简单，可同时估计噪声的均值和方差，被广泛的应用于各类自适应滤波器中。
所述系统模型如公式(5-1)所示，具有如公式(5-2)所示的噪声统计特性，在噪声统计特性未知或不准确的情况下，基于Sage-Husa算法可得次优常值噪声估计器：
其中是j-1时刻的状态估计值经过非线性状态函数f(·)传播后得到的状态预测值，是j时刻的状态估计值经过非线性量测函数h(·)传播后得到的量测预测值，在容积Kalman滤波算法中有
Xi,j-1|j-1与Xi,j|j-1分别表示j-1与j时刻状态估计值与计算得到的容积点。采用MAP估计器，得到噪声统计特性未知的无偏估计器，结合容积Kalman滤波算法，可得无偏常值噪声估计器：
其中表示新息向量。
进一步的，所述系统模型如公式(5-1)所示，公式(5-2)为其噪声统计特性，如果其噪声为时变的情况，需要减弱或消除陈旧量测信息，加大最近量测信息的作用，因此对求和式中的每一项引入不同的加权系数；采用渐消记忆指数加权的方式对时变噪声进行估计，可得无偏时变噪声估计器：
其中dk-1＝(1-b)/(1-bk)，b为遗忘因子，用来限制滤波器的记忆长度，通常b的取值范围为0.95＜b＜0.99，b的值越大越加重最近量测信息的作用，当噪声变化快时，b的取值应偏大，反之则偏小。
具体地，由公式(5-18)-(5-20)可知，通过MAP估计器得到的两个噪声估计器之间不是完全独立无关的，而且非独立的，因此必须已知系统噪声ωk和量测噪声υk中的一个才能对未知噪声进行估计修正，否则会引起滤波发散。
进一步的，所述系统模型形如公式(5-1)所示，在实际应用中，量测噪声容易受到量测误差和环境扰动等方面的影响，因此在公式(5-2)所示的噪声统计特性中，系统噪声ωk为均值为qk，方差为Qk的高斯白噪声序列，量测噪声υk也为高斯白噪声序列，但是统计特性未知；
结合Sage-Husa算法，首先采用局部估计的方式，对未知的量测噪声统计特性进行局部估计，然后再根据两阶段方法，对滤波器进行分解；在这个过程中，因使用的是平方根容积Kalman滤波算法，避免了误差协方差Pk|k-1失去正定性导致的滤波发散，不需要进行发散判断，即可保持滤波的正常递推进行。
进一步的，所述在两阶段容积Kalman滤波的过程中，整体的对噪声未知统计特性进行估计，将估计出的噪声统计特性作为已知条件进行递推估计具体为：
局部估计自适应两阶段平方根容积Kalman滤波算法中均以上角标G标明以示区分，算法步骤如下：
所述在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环具体为：
局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF-L)中均以上角标L标明以示区分，算法步骤如下：
初始化：
Step1时间更新：
⑴计算容积点Xi,k-|k-1和通过状态转移函数计算得到的传播容积点
⑵由于系统噪声均值不为0，故有
其中
⑶估计k时刻的状态预测值和
得到
⑷估计k时刻的状态误差协方差平方根因子
其中 定义如公式(5-25)所示：
⑸估计k时刻的状态误差协方差和
其中
⑹借助Uk计算和其中
Step2量测更新：
⑴计算容积点Xi，k|k-1和通过观测函数计算得到的传播容积点
⑵由于测量噪声的均值也不为0，估计k时刻的量测预测值
其中
⑶估计k时刻的量测误差协方差的平方根因子
其中
⑷估计k时刻的量测误差协方差
⑸估计k时刻的交叉协方差
⑹估计卡尔曼增益和其中
⑺计算k时刻状态估计值和
⑻计算k时刻状态误差协方差估计值和Vk；
Step3噪声估计：
⑴分别估计k时刻状态噪声的均值和
由公式(5-18)可得
即
得到
⑵分别估计k时刻状态噪声的方差由公式(5-19)可得
其中即
得到
Step4：如果k<N，N为跟踪时间，转到Step1。
进一步的，在局部估计自适应两阶段平方根容积Kalman滤波算法中，计算系统噪声统计特性时，通过得到系统噪声统计特性，并在两个阶段滤波中各自的表示方法，然后采用Sage-Husa滤波器分别进行估计，将估计出来的统计特性值加入到下一个时刻的递推估计中。
本发明实施例提供的技术方案带来的有益效果是：
本发明通过采用Saga-Husa估计器实时估计噪声的未知统计特性，再将实时更新的统计特性带入Kalman滤波算法，改善噪声统计特性未知系统中的滤波估计效果，再用新得到的系统状态和误差协方差再次修正噪声统计特征，以此形成递推循环；在局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF-G) 中，对于量测噪声统计特性的估计是采用Sage-Husa滤波器直接进行估计，然后将估计出来的统计特性值作为已知条件，从而提高了滤波的跟踪精度，使噪声的估计值逐步趋向于真实值，具有自适应的能力。
以上仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种局部估计自适应两阶段平方根容积滤波的方法，其特征在于，包括：
首先推导出采用误差协方差矩阵的平方根代替协方差矩阵参加递推运算的平方根两阶段容积Kalman滤波算法；
然后基于Sage-Husa滤波算法，提出自适应两阶段平方根容积Kalman滤波算法；
在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环。
2.如权利要求1所述的方法，其特征在于，所述平方根两阶段容积Kalman滤波算法中的系统模型具体为；
带有随机偏差的非线性高斯系统模型可表示为：
其中k是离散时间序列，xk∈Rn×1是系统的状态向量，bk∈Rp×1是系统偏差向量，zk∈Rm×1是量测向量，fk(·)和hk(·)为已知的非线性状态转移函数和量测函数且在xk处连续可微，是系统噪声序列、是偏差噪声序列，υk是量测噪声序列，均为高斯白噪声序列；其中均值为E(υk)＝r，方差和Rk满足如下条件：
令
Hk(Xk)＝hk(xk)+Fkbk
系统模型可以改写为如下形式：
Xk+1＝Fk(Xk)+ωk
Zk＝Hk(Xk)+υk
其统计特性满足如下公式(5-2)所示：
初始状态x0、b0与ωk、υk无关，且满足
3.如权利要求2所述的方法，其特征在于，所述平方根两阶段容积Kalman滤波算法步骤如下：
Step1时间更新:
⑴计算容积点Xi,k-|k-1和通过状态转移函数计算得到的传播容积点
⑵估计k时刻的状态预测值和
⑶估计k时刻的状态误差协方差平方根因子
其中 定义如公式(5-4)所示：
⑷借助Uk估计k时刻的状态误差协方差和其中
Step2量测更新:
⑴计算容积点Xi,k|k-1和通过观测函数计算得到的传播容积点Zi,k|k-1；
⑵估计k时刻的量测预测值
⑶估计k时刻量测误差协方差的平方根因子
Szz,k|k-1＝Tria([∈k|k-1SR,k]) (5-5)
其中∈k|k-1定义如公式(5-4)所示：
⑷估计k时刻的量测误差协方差
⑸估计k时刻的交叉协方差
⑹估计卡尔曼增益和其中
⑺计算k时刻状态估计值和
⑻计算k时刻状态误差协方差估计值 和Vk。
4.如权利要求3所述的方法，其特征在于，所述系统模型如公式(5-1)所示，具有如公式(5-2)所示的噪声统计特性，在噪声统计特性未知或不准确的情况下，基于Sage-Husa算法可得次优常值噪声估计器：
其中是j-1时刻的状态估计值经过非线性状态函数f(·)传播后得到的状态预测值，是j时刻的状态估计值经过非线性量测函数h(·)传播后得到的量测预测值，在容积Kalman滤波算法中有
Xi,j-1|j-1与Xi,j|j-1分别表示j-1与j时刻状态估计值与计算得到的容积点；采用MAP估计器，得到噪声统计特性未知的无偏估计器，结合容积Kalman滤波算法，可得无偏常值噪声估计器：
其中表示新息向量。
5.如权利要求4所述的方法，其特征在于，所述系统模型如公式(5-1)所示，公式(5-2)为其噪声统计特性，如果其噪声为时变的情况，需要减弱或消除陈旧量测信息，加大最近量测信息的作用，因此对求和式中的每一项引入不同的加权系数；采用渐消记忆指数加权的方式对时变噪声进行估计，可得无偏时变噪声估计器：
其中dk-1＝(1-b)/(1-bk)，b为遗忘因子，用来限制滤波器的记忆长度，通常b的取值范围为0.95<b<0.99，b的值越大越加重最近量测信息的作用，当噪声变化快时，b的取值应偏大，反之则偏小。
6.如权利要求5所述的方法，其特征在于，系统噪声受系统模型、人为操作等方面的影响，其统计特性具有未知性和时变性，因此在公式(5-2)所示的噪声统计特性中，量测噪声υk为均值为rk，方差为Rk的高斯白噪声序列，系统噪声υk也为高斯白噪声序列，但是统计特性未知。
7.如权利要求6所述的方法，其特征在于，所述在两个阶段的滤波器中分别估计该阶段的统计特性未知的噪声，然后分别改善各自滤波器的估计效果，再用新得到的系统状态和误差协方差分别再次修正各自滤波器的噪声统计特征，形成两个滤波器各自的递推循环具体为：
局部估计自适应两阶段平方根容积Kalman滤波算法(ATSCKF-L)中均以上角标L标明以示区分，算法步骤如下：
初始化：
Step1时间更新：
⑴计算容积点Xi,k-|k-1和通过状态转移函数计算得到的传播容积点
⑵由于系统噪声均值不为0，故有
其中
得到
⑷估计k时刻的状态误差协方差平方根因子
其中 定义如公式(5-25)所示：
其中
⑹借助Uk计算和其中
Step2量测更新：
⑴计算容积点Xi，k|k-1和通过观测函数计算得到的传播容积点
⑶估计k时刻的量测误差协方差的平方根因子
其中
⑷估计k时刻的量测误差协方差
⑸估计k时刻的交叉协方差
⑹估计卡尔曼增益和其中
⑻计算k时刻状态误差协方差估计值 和Vk；
Step3噪声估计：
⑴分别估计k时刻状态噪声的均值和
由公式(5-18)可得
即
得到
⑵分别估计k时刻状态噪声的方差
由公式(5-19)可得
得到
Step4：如果k<N，N为跟踪时间，转到Step1。
8.如权利要求7所述的方法，其特征在于，在局部估计自适应两阶段平方根容积Kalman滤波算法中，计算系统噪声统计特性时，通过得到系统噪声统计特性，并在两个阶段滤波中各自的表示方法，然后采用Sage-Husa滤波器分别进行估计，将估计出来的统计特性值加入到下一个时刻的递推估计中。
CN201810189568.0A 2018-03-08 2018-03-08 局部估计自适应两阶段平方根容积滤波的方法 Pending CN108446256A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810189568.0A CN108446256A (zh) 2018-03-08 2018-03-08 局部估计自适应两阶段平方根容积滤波的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810189568.0A CN108446256A (zh) 2018-03-08 2018-03-08 局部估计自适应两阶段平方根容积滤波的方法

Publications (1)

Publication Number Publication Date
CN108446256A true CN108446256A (zh) 2018-08-24

Family

ID=63193778

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810189568.0A Pending CN108446256A (zh) 2018-03-08 2018-03-08 局部估计自适应两阶段平方根容积滤波的方法

Country Status (1)

Country Link
CN (1) CN108446256A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109362049A (zh) * 2018-10-09 2019-02-19 中国人民解放军海军航空大学 基于混合信息加权一致的平方根容积滤波方法
CN109543143A (zh) * 2019-01-28 2019-03-29 杭州电子科技大学 非线性带偏差系统的多传感器融合估计方法
CN109871508A (zh) * 2019-03-14 2019-06-11 杭州电子科技大学 存在相关测量噪声的集中式两阶段卡尔曼估计方法
CN112255545A (zh) * 2019-07-05 2021-01-22 西南科技大学 基于平方根扩展卡尔曼滤波的锂电池soc估计模型
CN113472318A (zh) * 2021-07-14 2021-10-01 青岛杰瑞自动化有限公司 一种顾及观测模型误差的分级自适应滤波方法及系统

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109362049A (zh) * 2018-10-09 2019-02-19 中国人民解放军海军航空大学 基于混合信息加权一致的平方根容积滤波方法
CN109362049B (zh) * 2018-10-09 2021-09-03 中国人民解放军海军航空大学 基于混合信息加权一致的平方根容积滤波方法
CN109543143A (zh) * 2019-01-28 2019-03-29 杭州电子科技大学 非线性带偏差系统的多传感器融合估计方法
CN109543143B (zh) * 2019-01-28 2023-08-18 杭州电子科技大学 非线性带偏差系统的多传感器融合估计方法
CN109871508A (zh) * 2019-03-14 2019-06-11 杭州电子科技大学 存在相关测量噪声的集中式两阶段卡尔曼估计方法
CN112255545A (zh) * 2019-07-05 2021-01-22 西南科技大学 基于平方根扩展卡尔曼滤波的锂电池soc估计模型
CN113472318A (zh) * 2021-07-14 2021-10-01 青岛杰瑞自动化有限公司 一种顾及观测模型误差的分级自适应滤波方法及系统
CN113472318B (zh) * 2021-07-14 2024-02-06 青岛杰瑞自动化有限公司 一种顾及观测模型误差的分级自适应滤波方法及系统

Similar Documents

Publication Publication Date Title
CN108446256A (zh) 局部估计自适应两阶段平方根容积滤波的方法
CN108509379A (zh) 全局估计自适应两阶段平方根容积滤波的方法
Wang et al. Iterated maximum correntropy unscented Kalman filters for non-Gaussian systems
CN107728138B (zh) 一种基于当前统计模型的机动目标跟踪方法
CN109163720A (zh) 基于渐消记忆指数加权的卡尔曼滤波跟踪方法
CN103927436A (zh) 一种自适应高阶容积卡尔曼滤波方法
CN108981696B (zh) 一种sins任意失准角无奇异快速传递对准方法
CN108061887A (zh) 一种基于模糊交互式多模型算法的临近空间目标跟踪方法
CN109582916A (zh) 一种基于观测噪声方差未知的自适应卡尔曼滤波方法
CN112068092B (zh) 一种用于高精度弹道实时定轨的抗差加权观测融合平方根ukf滤波方法
CN108120452B (zh) Mems陀螺仪动态数据的滤波方法
CN109840517A (zh) 一种mems陀螺噪声估计和滤波方法
CN108413986A (zh) 一种基于Sage-Husa卡尔曼滤波的陀螺仪滤波方法
CN111623779A (zh) 一种适用于噪声特性未知的时变系统自适应级联滤波方法
CN112986977B (zh) 一种克服雷达扩展卡尔曼航迹滤波发散的方法
CN103793614B (zh) 一种突变滤波方法
Fu et al. Maneuvering target tracking with improved unbiased FIR filter
Luo et al. Target tracking based on amendatory Sage-Husa adaptive Kalman filtering
CN109582915B (zh) 应用于纯方位跟踪的改进非线性可观测度自适应滤波方法
CN115580261A (zh) 基于鲁棒策略和动态增强ckf结合的状态估计方法
Rui K-means aided Kalman Filter noise estimation calibration for integrated GPS/INS Navigation
Liu et al. Effective Sage-Husa Kalman filter for SINS/Doppler/Platform compass integrated navigation system
CN108225373B (zh) 一种基于改进的5阶容积卡尔曼的大失准角对准方法
CN113376672A (zh) 基于模糊卡尔曼滤波的北斗伪距定位算法
CN113432608A (zh) 适于ins/cns组合导航系统的基于最大相关熵的广义高阶ckf算法

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20180824