CN109000663A - 基于时变自调整的滚动时域估计到达代价计算方法 - Google Patents
基于时变自调整的滚动时域估计到达代价计算方法 Download PDFInfo
- Publication number
- CN109000663A CN109000663A CN201810888945.XA CN201810888945A CN109000663A CN 109000663 A CN109000663 A CN 109000663A CN 201810888945 A CN201810888945 A CN 201810888945A CN 109000663 A CN109000663 A CN 109000663A
- Authority
- CN
- China
- Prior art keywords
- time
- state
- variance
- arrival cost
- varying
- 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
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 27
- 238000000034 method Methods 0.000 claims abstract description 23
- 238000005096 rolling process Methods 0.000 claims description 26
- 239000011159 matrix material Substances 0.000 claims description 13
- 238000005259 measurement Methods 0.000 claims description 6
- 238000005562 fading Methods 0.000 claims description 4
- 230000014509 gene expression Effects 0.000 claims description 4
- 238000001914 filtration Methods 0.000 description 11
- 238000005070 sampling Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000008033 biological extinction Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
Classifications
-
- 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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明属于飞行器姿态估计领域,针对滚动时域估计中现有计算到达代价的方法精度不高,鲁棒性不强的问题,本发明设计了一种基于时变自调整的滚动时域估计到达代价计算方法。首先,为了提高姿态估计精度,设计了四元数MEMS陀螺/TAM组合姿态估计模型,以弥补MEMS陀螺单独姿态估计精度低的缺点;然后,针对TAM在线工作易受外界干扰的特点,设计了基于时变自调整的到达代价求解方法以增强系统鲁棒性。
Description
技术领域
本发明属于飞行器姿态估计滤波技术领域,更具体地,本发明涉及一种滚动时域估计滤波的时变自调整到达代价计算方法,可用于强干扰下的飞行器姿态估计。
背景技术
在过去的几十年里,基于优化的滚动时域估计(Moving Horizon Estimation,MHE)在非线性约束问题中应用非常广泛。和全信息估计相比,MHE通过仅考虑过去有限量测值来减小计算负担。过去历史信息对当前状态的影响即为到达代价。对于线性无约束姿态估计系统,标准卡尔曼滤波方差更新公式可以用来计算到达代价,但是对于非线性有约束姿态估计系统,到达代价的通用解析表达式不再适用。
对于非线性系统的到达代价的计算方法,目前主要研究方向都是针对EKF需要泰勒级数展开精度不高以及雅可比矩阵计算较为复杂的问题进行讨论。然而,以上这些文献都没有考虑算法的鲁棒性。事实上,由于传感器实际工作状态与线下状态有很大差别,极端的温度、压力和电磁环境使实际系统的参数发生变动,造成原来比较准确的模型也与实际系统不再匹配。同时,由于受到外界电磁信号的干扰,所建模型的噪声统计特性与实际噪声的统计特性可能有较大的差异。所有这些因素,包括对初始状态的不准确估计,都会影响滤波器的精度。因而,寻找一种鲁棒性强的到达代价计算方法在滤波估计系统中至关重要。
带有时变自调整因子的滤波器可以克服这些问题。它是一种效果良好的强跟踪滤波器,可以在线调整滤波增益,从而达到算法自适应的目的。不仅具有极强的模型失配鲁棒性,且具有概念清晰、计算简单的优点。将时变自调整因子滤波器的自适应性与UKF的非线性高逼近性结合起来,使得本方法具有较强的自适应能力和鲁棒性。
发明内容
本发明的目的在于突破传统滤波算法在抗干扰能力的不足,提高系统鲁棒性,提高系统自适应能力,提出一种基于强跟踪的滚动时域估计到达代价计算方法。其中解决的主要问题包括:
(1)对于非线性系统的到达代价的计算方法,目前主要研究方向都是针对EKF需要泰勒级数展开精度不高以及雅可比矩阵计算较为复杂的问题进行讨论。没有考虑算法的鲁棒性。
(2)由于传感器实际工作状态与线下状态有很大差别,极端的温度、压力和电磁环境使实际系统的参数发生变动,造成原来比较准确的模型也与实际系统不再匹配。
(3)由于较容易受到外界电磁信号的干扰,所建模型的噪声统计特性与实际噪声的统计特性可能有较大的差异。
(4)对初始状态的不准确估计,会影响滤波器的精度。
本发明所述的基于时变自调整的滚动时域估计到达代价计算方法,其特征在于包括以下技术措施:
步骤一:利用状态方程、观测方程的线性化矩阵,加入渐消因子,计算时变自调整因子,以实现对到达代价的时变自调整;
步骤二:在得到时变自调整因子的基础上,利用状态方程及观测方程,计算带有时变自调整因子的系统状态及方差的粗略估计值;
步骤三:在得到状态及方差的粗略估计值,利用带有时变自调整因子的协方差计算式,计算下一时刻状态及方差粗略估计值精调整值;
步骤四:将上步中输出的下一时刻状态及方差粗略估计值精调整值带入MHE系统计算到达代价。
对比现有技术,本技术方案所述的基于时变自调整因子的滚动时域估计到达代价计算方法,有益效果在于:
(1)本方法通过时变自调整因子无迹卡尔曼滤波计算滚动时域估计问题的到达代价,充分考虑了算法的鲁棒性;
(2)当传感器实际工作状态与线下差别较大,系统参数发生变化时,计算误差较小;
(3)当受到外界电磁信号干扰时,具有较强的抵抗能力,所建模型的噪声统计特性与实际特性差异大时能够自适应修正;
(4)可以修正对初始状态的不准确估计。
附图说明
附图1是基于时变自调整因子的滚动时域估计到达代价计算方法实施流程图。
附图2是滚动时域估计滤波方法基本原理图。
附图3是滚动时域估计滤波计算方法流程图。
具体实施方式
为了解决滚动时域估计方法鲁棒性不强,抗干扰能力较差的问题,提出了基于时变自调整因子的到达代价计算方法。首先,设计了时变自调整次优调节因子的计算方法,然后将该方法带入到UKF滤波算法中,计算滚动时域估计“窗口”外的状态估计值以及方差,最后将得到的估计值和方差带入到滚动时域估计系统中计算到达代价。
以下结合说明书附图1对本发明做进一步的详细描述。参照说明书附图1,本发明的处理流程分以下步骤:
(1)系统开始状态的赋值
(2)针对n维状态向量,根据中心扩展采样法选取代表点并计算加权数
假设状态向量x为n维,选取2n+1个采样点xk,p0,p0=0,...,2n。中心扩展采样法规则如下:
加权数计算方法如下:
其中,ρ0=τ2(n+κ)-n,τ通常取一个很小的数,对于高斯先验分布的情形μ=2最优,κ通常取0。
(3)时变自调整次优调节因子的计算
时变自调整次优调节滤波器的一个充分条件是在线选择一个适当的时变增益矩阵使得残差正交。当滤波器状态估值偏离系统的状态时,结果会在残差序列的均值和幅值上表现出来。这时,可以通过增加时变次优调节因子Λ来实时调整状态预报误差协方差,强迫残差序列仍然保持正交,从而达到调整滤波增益矩阵形成强跟踪滤波器的目的。时变滤波增益矩阵中的次优调节因子:
其中
Λ0=trNk/trMk
Fk是状态方程的线性化矩阵,在算法中形式是Hk是观测方程的线性化矩阵,在算法中的形式为tr(·)为矩阵的迹;
0≤ρ≤1为渐消因子,通常取值0.95。引入渐消因子的目的是为了进一步提高滤波器的快速跟踪能力。滤波的实际误差不应小于理论误差,且不同的应用场合对系统的跟踪性能和滤波性能的侧重不同。Φ为减小系数,一般取经验值。
(4)下一时刻状态及方差的粗略估计值的计算
χp0,k|k-1=f(xp0,k-1),p0=0,...,2n
γp0,k|k-1=h(χp0,k|k-1)
(5)下一时刻状态及方差的粗略估计值精调整
(6)滚动时域估计算法到达代价的计算
滚动时域估计算法的基本思想为,只考虑当前时刻以前固定数量的数据,同时将历史数据对估计的影响用一种方法来近似描述,则每次参与优化的数据量不变,这种方法实现起来就像一个滚动的观测“窗口”。这个“窗”包含过去有限的量测值,“窗”的最后一个输出为当前状态估计,附图2为滚动时域估计滤波方法基本原理图。
设窗口长度为L(i=1,2,..,L),其中窗口最后的一个点(i=L)代表现在时刻k。滚动时域估计问题可表示为
s.t.
xi+1=f(xi,ui)+wi,for i=1,…,L-1 (3b)
yi=h(xi)+vi,for i=2,…,L (3c)
li(xi)=0,for i=1,…,L (3d)
其中xi是待估计的状态。式(3b)描述了在离散情况下的状态递推过程。在该递推方程中ui是已知输入,它的过程噪声为wi,过程噪声方差为Qi。方程(3c)为量测方程,yi为量测值,vi、Ri分别为量测噪声和量测噪声方差。在时域内的第一个时刻点为方差为P1 0。(3a)第三项θk为代价函数,它描述了到达代价。值得注意的是i=1时刻的量测值并没有包含在代价函数中。假定从量测中得到的所有信息已经包含在先验估计中。等式约束由式(3d)给出。滚动时域估计问题的输出为xL:时域“窗”中最后一个时刻的估计状态向量。
将式(1)、(2)带入到滚动时域估计系统(3e)中就可以得到到达代价θk。附图3是滚动时域估计滤波计算方法流程图。
Claims (5)
1.基于时变自调整的滚动时域估计到达代价计算方法,其特征在于,包括以下技步骤:
步骤一:利用状态方程、观测方程的线性化矩阵,加入渐消因子,计算时变自调整因子,以实现对到达代价的时变自调整;
步骤二:在得到时变自调整因子的基础上,利用状态方程及观测方程,计算带有时变自调整因子的系统状态及方差的粗略估计值;
步骤三:在得到状态及方差的粗略估计值,利用带有时变自调整因子的协方差计算式,计算下一时刻状态及方差粗略估计值精调整值;
步骤四:将上步中输出的下一时刻状态及方差粗略估计值精调整值带入MHE系统计算到达代价。
2.根据权利1要求所述基于时变自调整的滚动时域估计到达代价计算方法,其特征在于,步骤一中时变自调整因子的计算方法为:
其中
Λ0=trNk/trMk
Fk是k时刻状态方程的线性化矩阵,Hk是观测方程的线性化矩阵,tr(·)为矩阵的迹,0≤ρ≤1为渐消因子,取值0.95,Φ为减小系数,过程噪声方差为Qk,Rk是量测噪声方差,状态方差为Pk,εk是状态残差。
3.根据权利2要求所述基于时变自调整的滚动时域估计到达代价计算方法,其特征在于,步骤二中带有时变自调整因子的系统状态及方差的粗略估计值的计算方法为:
其中,
χp0,k|k-1=f(xp0,k-1),p0=0,...,2n
γp0,k|k-1=h(χp0,k|k-1)
为权重系数,f(·)为系统状态方程,h(·)为系统量测方程,为第k时刻的状态预测值,xp0,k-1表示k-1时刻第p0个状态采样点。
4.根据权利3要求所述基于时变自调整的滚动时域估计到达代价计算方法,其特征在于,步骤三中下一时刻状态及方差粗略估计值精调整值计算方法为:
其中,
5.根据权利4要求所述基于强跟踪的滚动时域估计到达代价计算方法,其特征在于,步骤四中将上步中下一时刻状态及方差粗略估计值精调整值带入MHE系统计算到达代价方法为:
其中,窗口长度为L(i=1,2,..,L),其中窗口最后的一个点(i=L)是现在时刻k,在窗口内的第一个时刻状态为方差为P1 0,θk为到达代价,将式(1)、(2)带入到式(3)中就得到到达代价θk。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810888945.XA CN109000663A (zh) | 2018-08-07 | 2018-08-07 | 基于时变自调整的滚动时域估计到达代价计算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810888945.XA CN109000663A (zh) | 2018-08-07 | 2018-08-07 | 基于时变自调整的滚动时域估计到达代价计算方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109000663A true CN109000663A (zh) | 2018-12-14 |
Family
ID=64595934
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810888945.XA Pending CN109000663A (zh) | 2018-08-07 | 2018-08-07 | 基于时变自调整的滚动时域估计到达代价计算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109000663A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111578931A (zh) * | 2020-05-21 | 2020-08-25 | 中国人民解放军海军航空大学 | 基于在线滚动时域估计的高动态飞行器自主姿态估计方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102998629A (zh) * | 2012-12-16 | 2013-03-27 | 天津大学 | 一种锂电池荷电状态的估计方法 |
CN104833967A (zh) * | 2015-05-11 | 2015-08-12 | 重庆大学 | 一种基于滚动时域估计的雷达目标跟踪方法 |
CN105572596A (zh) * | 2016-01-20 | 2016-05-11 | 上海交通大学 | 锂电池soc估算方法及系统 |
CN107153163A (zh) * | 2017-06-20 | 2017-09-12 | 福建工程学院 | 一种基于自适应ukf的锂电池soc估算方法 |
CN107219466A (zh) * | 2017-06-12 | 2017-09-29 | 福建工程学院 | 一种混合扩展卡尔曼滤波的锂电池soc估算方法 |
-
2018
- 2018-08-07 CN CN201810888945.XA patent/CN109000663A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102998629A (zh) * | 2012-12-16 | 2013-03-27 | 天津大学 | 一种锂电池荷电状态的估计方法 |
CN104833967A (zh) * | 2015-05-11 | 2015-08-12 | 重庆大学 | 一种基于滚动时域估计的雷达目标跟踪方法 |
CN105572596A (zh) * | 2016-01-20 | 2016-05-11 | 上海交通大学 | 锂电池soc估算方法及系统 |
CN107219466A (zh) * | 2017-06-12 | 2017-09-29 | 福建工程学院 | 一种混合扩展卡尔曼滤波的锂电池soc估算方法 |
CN107153163A (zh) * | 2017-06-20 | 2017-09-12 | 福建工程学院 | 一种基于自适应ukf的锂电池soc估算方法 |
Non-Patent Citations (1)
Title |
---|
赵国荣等: "基于滚动时域估计的飞行器姿态估计及三轴磁强计在线校正", 《物理学报》 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111578931A (zh) * | 2020-05-21 | 2020-08-25 | 中国人民解放军海军航空大学 | 基于在线滚动时域估计的高动态飞行器自主姿态估计方法 |
CN111578931B (zh) * | 2020-05-21 | 2022-03-04 | 中国人民解放军海军航空大学 | 基于在线滚动时域估计的高动态飞行器自主姿态估计方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107561503B (zh) | 一种基于多重渐消因子的自适应目标跟踪滤波方法 | |
CN107728138B (zh) | 一种基于当前统计模型的机动目标跟踪方法 | |
CN107390199B (zh) | 一种雷达机动目标跟踪波形设计方法 | |
CN109163720A (zh) | 基于渐消记忆指数加权的卡尔曼滤波跟踪方法 | |
CN105043388B (zh) | 基于惯性/重力匹配组合导航的向量搜索迭代匹配方法 | |
CN106443661A (zh) | 基于无迹卡尔曼滤波的机动扩展目标跟踪方法 | |
CN105301666B (zh) | 一种航磁干扰补偿系数的自适应调整方法 | |
CN105447574B (zh) | 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置 | |
CN107045490A (zh) | 一种非线性系统的状态估计方法 | |
CN108344409B (zh) | 提高卫星姿态确定精度的方法 | |
CN112432644B (zh) | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 | |
CN103776449B (zh) | 一种提高鲁棒性的动基座初始对准方法 | |
CN104504728A (zh) | 多机动目标跟踪方法、系统及其广义联合概率数据关联器 | |
CN109520503A (zh) | 一种平方根容积模糊自适应卡尔曼滤波slam方法 | |
CN115981148A (zh) | 无人机地面移动目标跟踪方法 | |
CN109341690A (zh) | 一种鲁棒高效的组合导航自适应数据融合方法 | |
CN109000663A (zh) | 基于时变自调整的滚动时域估计到达代价计算方法 | |
CN112986977B (zh) | 一种克服雷达扩展卡尔曼航迹滤波发散的方法 | |
CN107807906A (zh) | 一种多模型自校准秩滤波方法 | |
CN110567490B (zh) | 一种大失准角下sins初始对准方法 | |
CN109582915B (zh) | 应用于纯方位跟踪的改进非线性可观测度自适应滤波方法 | |
CN109375160B (zh) | 纯方位无源定位中一种测角误差估计方法 | |
Wang et al. | Optimal Weight and Parameter Estimation of Multi‐structure and Unequal‐Precision Data Fusion | |
CN115498980A (zh) | 一种基于M估计的递归最小p阶自适应滤波定位方法 | |
CN113758503B (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 | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20181214 |
|
RJ01 | Rejection of invention patent application after publication |