CN104868876B - 一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法 - Google Patents
一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法 Download PDFInfo
- Publication number
- CN104868876B CN104868876B CN201510239254.3A CN201510239254A CN104868876B CN 104868876 B CN104868876 B CN 104868876B CN 201510239254 A CN201510239254 A CN 201510239254A CN 104868876 B CN104868876 B CN 104868876B
- Authority
- CN
- China
- Prior art keywords
- mtd
- msub
- mtr
- mrow
- msubsup
- 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.)
- Active
Links
Landscapes
- Feedback Control In General (AREA)
- Navigation (AREA)
- Filters That Use Time-Delay Elements (AREA)
Abstract
本发明提供一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法,具体过程为:步骤一,判断系统模型是否满足预设条件,若满足,则进入步骤二,否则结束该方法;步骤二,构建系统广义输出矩阵E;步骤三,利用所述矩阵E确定系统输出的耦合向量δi;步骤四、根据耦合向量δi构建系统输出的线性耦合;基于所述线性耦合,利用大数定律对过程噪声的协方差矩阵Q进行估计,将估计的结果记为步骤五、将过程噪声的协方差矩阵的实时估计结果带入到标准Kalman滤波器中,获得滤波结果。本发明方法可以实现过程噪声协方差矩阵Q未知情况下的Kalman滤波。
Description
技术领域
本发明涉及一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法,本发明属于Kalman滤波方法技术领域。
背景技术
Kalman滤波是一种时域滤波方法,采用状态空间方法对系统进行描述,算法采用递推形式,数据存储量小,在计算机上执行起来十分方便。其不仅可以处理平稳随机过程,还可以处理多维非平稳随机过程。当系统模型及噪声先验信息已知而且噪声满足高斯白噪声假设的前提下,可以证明Kalman滤波结果是最小方差无偏估计。因为Kalman滤波方法简单易行,结果准确可靠,具有其他滤波方法所不具备的优点,所以Kalman滤波作为一种重要的最优估计方法广泛的应用于各种领域,如惯性导航、制导系统、全球定位系统、通信与信号过程、随机最优控制以及故障诊断等应用领域。
Kalman滤波是建立在模型精确、随机干扰信号统计特性已知基础上的,但是对于一个实际应用系统,其中往往存在着模型不确定或者噪声信号统计特性不完全已知的情况,尤其是很难提前获得准确的过程噪声信号统计特性,这是因为在实际应用的过程中,受到环境变化、器件老化和系统冲击等因素的影响,系统过程噪声的统计特性会发生较大变化,同时在系统实际运行过程中很难对噪声的统计特性进行实时测量。对于过程噪声而言,其统计特性直接由其协方差矩阵Q决定;过程噪声的协方差矩阵Q中的对角线元素代表了过程噪声的方差,非对角线元素代表了不同过程噪声之间的协方差。统计特性的不确定性将直接导致滤波算法中所使用的Q和真实的Q之间存在较大的偏差,而该偏差会使得Kalman滤波算法失去最优性,估计精度大大下降,严重时甚至会引起滤波发散。所以如何使得Kalman滤波算法在噪声信号统计特性不完全已知的情况下仍然能获得最优的滤波结果具有十分重要的意义。
发明内容
有鉴于此,本发明的目的在于克服已有技术存在的不足,提出一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法。
本发明的目的是通过下述技术方案实现
一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法,具体过程为:
步骤一,判断系统模型是否满足预设条件,若满足,则进入步骤二,否则结束该方法;
所述预设条件为:
其中,rank[]代表矩阵的秩,A为系统状态转移矩阵,H为系统观测矩阵,m为系统状态变量的个数;
步骤二,构建系统广义输出矩阵E;
步骤三,利用所述矩阵E确定系统输出的耦合向量δi;
步骤四、根据耦合向量δi构建系统输出的线性耦合;基于所述线性耦合,利用大数定律对过程噪声的协方差矩阵Q进行估计,将估计的结果记为
步骤五、将过程噪声的协方差矩阵的实时估计结果带入到标准Kalman滤波器中,获得滤波结果。
进一步地,本发明所述矩阵E定义为:
进一步地,本发明步骤四的具体过程为:
定义系统当前所处的时刻为l,同时定义其中代表向下取整;根据耦合向量δi计算输出的线性耦合
其中,Z1,Z2…Z2p分别对应时刻1,2,…,2p的系统输出;
利用大数定律对过程噪声的协方差矩阵Q进行估计:
其中,R为系统量测噪声的协方差矩阵;
本发明的有益效果:
本发明与标准Kalman滤波方法相比,该方法可以应用于过程噪声的协方差矩阵Q未知的情况下,扩展了标准Kalman滤波的使用范围;同时因为该方法并没有对Kalman滤波方法的滤波计算回路做出改动,所以该滤波方法的滤波结果仍然是最优的。
附图说明
图1为本发明滤波方法的流程图;
图2为本发明的具体实施例中的过程噪声w1k的方差估计值示意图;
图3为本发明的具体实施例中的过程噪声w2k的方差估计值示意图;
图4为本发明的具体实施例中的状态1的滤波估计误差示意图;
图5为本发明的具体实施例中的状态2的滤波估计误差示意图;
图6为本发明的具体实施例中的状态3的滤波估计误差示意图。
具体实施方式
下面结合附图和具体实例对本发明进行详细说明。
本发明的基本原理是:在系统模型满足一定条件的基础上,将系统量测噪声的协方差矩阵R作为已知条件,将不同时刻的系统输出进行线性耦合用于消去系统状态变量,同时利用大数定律对该耦合输出的协方差矩阵进行估计,进而准确地估计过程噪声的协方差矩阵Q,最后将Q的估计结果实时提供给Kalman滤波器完成滤波计算,保证了即使在Q未知的情况下,滤波结果也仍然能具有足够的准确性。
一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法,如图1所示,具体过程为:
步骤一、判断系统模型是否满足使用本发明方法的预设条件;
考虑系统离散化后的模型如式1所示:
其中,A为系统状态转移矩阵;H为系统观测矩阵;Xk+1和Xk分别代表第k+1步和第k步的系统状态;Zk代表系统第k步的观测值,下标k和k+1代表迭代步数,均为正整数;Bwk表示第k步的系统过程噪声;vk表示第k步的系统量测噪声;系统的过程噪声和量测噪声均为互不相关的高斯白噪声。
系统状态变量的个数为m,系统观测值的个数为n;则Bwk和vk可以进一步定义为如下形式:
对于过程噪声Bwk,其协方差矩阵为Q,其维数为m×m;对于量测噪声vk,其协方差矩阵为R,其维数为n×n;Q和R均为对角矩阵,其非对角线元素均为0,Q中的对角线元素依序为Q1,…,Qm。
基于此模型,给出该滤波方法的使用条件:
该方法的核心在于确定Q,即确定Q1,…,Qm。此处以求取为例进行说明。
定义向量则该滤波方法的使用条件可以表达为如下形式:
对于任意满足的i,均有下式成立:
其中,rank[]代表矩阵的秩,系统模型满足式(4)中的公式同时R已知,为该方法的使用条件。
步骤二、在满足本发明使用条件的基础上,构建系统广义输出矩阵E。
利用系统模型,可知如下矩阵等式成立:
本实施例中较佳的将矩阵定义为系统的广义输出矩阵,用E来代表。
步骤三、利用所述矩阵E确定耦合向量δi。
耦合向量δi需要使得如下方程成立:
由于该系统模型满足该方法的使用条件,所以该方程一定有解,即一定可以确定出δi满足式(6)。
步骤四、根据耦合向量δi构建系统输出的线性耦合,进而对过程噪声的协方差矩阵Q进行估计。
定义系统当前所处的时刻为l,同时定义其中代表向下取整;根据耦合向量δi计算输出的线性耦合
其中,Z1,Z2…Z2p分别对应时刻1,2,…,2p的系统输出。
利用式(7)中的可以进一步计算得到由大数定律可知,下述公式成立:
式(8)可以化简为如下形式:
由于δi及R均为已知,则可以对Qi进行实时估计。
令i依次等于1,…,m,则可以获取Q1,Q2,…,Qm,进一步可以对Q进行估计,估计结果为
步骤五、将过程噪声协方差矩阵的实时估计结果带入到标准Kalman滤波器中,获得滤波结果。
定义通过上述方法实时估计得到的过程噪声协方差矩阵为令其替代标准Kalman滤波过程中的Q:
Kk=Pk,k-1HT[HPk,k-1HT+R]-1 (13)
Pk=[I-KkH]Pk,k-1 (15)
其中,代表上一步滤波结果,代表状态一步预测,Pk,k-1代表一步预测误差方差,Kk代表滤波增益,Pk代表当前步滤波误差方差,Xk代表状态真值,ΔXk代表滤波误差。
通过上述步骤,便可以完成对系统状态的最优估计。
为了验证上述自适应滤波方法的准确性,本发明利用Matlab对该方法进行仿真验证,证明了该方法的有效性和准确性。
假设某离散系统模型如下所示:
其中T=0.01,过程噪声的协方差矩阵为量测噪声的协方差矩阵
在Kalman滤波过程中,除过程噪声协方差矩阵Q以外,系统模型的其他参数均为已知。
对该系统模型进行分析,可以发现该系统模型可以满足该滤波方法的使用条件,可以利用该方法对w1k和w2k的方差进行准确地实时估计。
w1k的方差估计结果如附图2所示,w2k的方差估计结果如附图3所示。采用该滤波方法对系统状态进行滤波,状态1的滤波误差如附图4所示,状态2的滤波误差如附图5所示,状态3的滤波误差如附图6所示。
从附图2和附图3中可以看出,该方法随着系统输出数目的增多,该方法估计得到w1k的方差收敛于2,估计得到的w2k的方差收敛于3,而且估计结果均很快的收敛到了相对应的真值附近,并且随着仿真步数的增加而逐渐趋向于真值,则可以说明该方法可以对w1k的方差和w2k的方差做出准确的估计。同时附图4、附图5和附图6说明,采用该方法对Q未知情况下的系统进行滤波,可以取得准确的滤波结果,并可以有效的限制滤波发散并提升滤波精度。
该结论证实了该滤波方法的准确性和其在实际工程上的使用价值。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进,或者对其中部分技术特征进行等同替换,这些改进和替换也应视为本发明的保护范围。
Claims (2)
1.一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法,该方法所针对的系统量测噪声的协方差矩阵R已知,其特征在于,具体过程为:
步骤一,判断系统模型是否满足预设条件,若满足,则进入步骤二,否则结束该方法;
所述预设条件为:
其中,rank[]代表矩阵的秩,A为系统状态转移矩阵,H为系统观测矩阵,m为系统状态变量的个数;
步骤二,构建系统广义输出矩阵E;
<mrow>
<mi>E</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>H</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>H</mi>
<mi>A</mi>
</mrow>
</mtd>
<mtd>
<mi>H</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
步骤三,利用所述矩阵E确定系统输出的耦合向量δi;
步骤四、根据耦合向量δi构建系统输出的线性耦合;基于所述线性耦合,利用大数定律对过程噪声的协方差矩阵Q进行估计,将估计的结果记为
所述根据耦合向量δi构建系统输出的线性耦合的过程如下:
定义系统当前所处的时刻为l,同时定义其中代表向下取整;根据耦合向量δi计算输出的线性耦合
<mrow>
<msubsup>
<mover>
<mi>Z</mi>
<mo>~</mo>
</mover>
<mn>1</mn>
<mi>i</mi>
</msubsup>
<mo>=</mo>
<msub>
<mi>&delta;</mi>
<mi>i</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<msubsup>
<mover>
<mi>Z</mi>
<mo>~</mo>
</mover>
<mn>2</mn>
<mi>i</mi>
</msubsup>
<mo>=</mo>
<msub>
<mi>&delta;</mi>
<mi>i</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mn>4</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mo>...</mo>
<mo>...</mo>
<mo>,</mo>
<msubsup>
<mover>
<mi>Z</mi>
<mo>~</mo>
</mover>
<mi>p</mi>
<mi>i</mi>
</msubsup>
<mo>=</mo>
<msub>
<mi>&delta;</mi>
<mi>i</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mrow>
<mn>2</mn>
<mi>p</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>Z</mi>
<mrow>
<mn>2</mn>
<mi>p</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中,Z1,Z2…Z2p分别对应时刻1,2,…,2p的系统输出;
步骤五、将过程噪声的协方差矩阵的实时估计结果带入到标准Kalman滤波器中,获得滤波结果。
2.根据权利要求1所述针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法,其特征在,所述协方差矩阵Q进行估计的具体过程为:
利用大数定律对过程噪声的协方差矩阵Q进行估计:
<mrow>
<msub>
<mi>Q</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mi>p</mi>
</mfrac>
<mo>&lsqb;</mo>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mover>
<mi>Z</mi>
<mo>~</mo>
</mover>
<mn>1</mn>
<mi>i</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mover>
<mi>Z</mi>
<mo>~</mo>
</mover>
<mn>2</mn>
<mi>i</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<mo>...</mo>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mover>
<mi>Z</mi>
<mo>~</mo>
</mover>
<mi>p</mi>
<mi>i</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>&rsqb;</mo>
<mo>-</mo>
<msub>
<mi>&delta;</mi>
<mi>i</mi>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>R</mi>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mi>R</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<msubsup>
<mi>&delta;</mi>
<mi>i</mi>
<mi>T</mi>
</msubsup>
</mrow>
其中,R为系统量测噪声的协方差矩阵;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510239254.3A CN104868876B (zh) | 2015-05-12 | 2015-05-12 | 一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510239254.3A CN104868876B (zh) | 2015-05-12 | 2015-05-12 | 一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104868876A CN104868876A (zh) | 2015-08-26 |
CN104868876B true CN104868876B (zh) | 2017-10-03 |
Family
ID=53914443
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510239254.3A Active CN104868876B (zh) | 2015-05-12 | 2015-05-12 | 一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104868876B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107290742B (zh) * | 2017-06-20 | 2019-06-11 | 武汉理工大学 | 一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法 |
CN110865334B (zh) * | 2019-11-26 | 2021-09-03 | 北京航空航天大学 | 一种基于噪声统计特性的多传感器目标跟踪方法及系统 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102005004568A1 (de) * | 2005-02-01 | 2006-08-10 | Robert Bosch Gmbh | Verfahren zur Berücksichtigung von Messwerten von kalibrierten Sensoren in einme Kalmanfilter |
CN101853243A (zh) * | 2010-04-01 | 2010-10-06 | 西北工业大学 | 系统模型未知的自适应卡尔曼滤波方法 |
GR1007449B (el) * | 2011-01-14 | 2011-11-07 | Δημητριος Χρηστου Μπουρλης | Μεθοδος εκτιμησης συμμεταβλητοτητας του θορυβου διεργασιας για προσαρμοστικο φιλτρο καλμαν σε μεταβαλλομενες συνθηκες θορυβου μετρησης και θορυβου διεργασιας |
CN104202019A (zh) * | 2014-08-25 | 2014-12-10 | 北京理工大学 | 带有未知过程噪声协方差阵递推估计的卡尔曼滤波方法 |
-
2015
- 2015-05-12 CN CN201510239254.3A patent/CN104868876B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102005004568A1 (de) * | 2005-02-01 | 2006-08-10 | Robert Bosch Gmbh | Verfahren zur Berücksichtigung von Messwerten von kalibrierten Sensoren in einme Kalmanfilter |
CN101853243A (zh) * | 2010-04-01 | 2010-10-06 | 西北工业大学 | 系统模型未知的自适应卡尔曼滤波方法 |
GR1007449B (el) * | 2011-01-14 | 2011-11-07 | Δημητριος Χρηστου Μπουρλης | Μεθοδος εκτιμησης συμμεταβλητοτητας του θορυβου διεργασιας για προσαρμοστικο φιλτρο καλμαν σε μεταβαλλομενες συνθηκες θορυβου μετρησης και θορυβου διεργασιας |
CN104202019A (zh) * | 2014-08-25 | 2014-12-10 | 北京理工大学 | 带有未知过程噪声协方差阵递推估计的卡尔曼滤波方法 |
Non-Patent Citations (2)
Title |
---|
Identification of process and measurement noise covariance for state and parameter estimation using extended Kalman filter;VA Bavdekar et al;《Journal of Process Control》;20111231;第21卷(第4期);第585-601页 * |
Kalman Filter With Recursive Covariance Estimation-Sequentially Estimating Process Noise Covariance;B Feng et al;《IEEE Transactions on Industrial Electronics》;20141231;第61卷(第11期);第6253-6263页 * |
Also Published As
Publication number | Publication date |
---|---|
CN104868876A (zh) | 2015-08-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN102721951B (zh) | 一种高机动目标跟踪方法 | |
CN105549049B (zh) | 一种应用于gps导航的自适应卡尔曼滤波算法 | |
CN106646356A (zh) | 一种基于卡尔曼滤波定位的非线性系统状态估计方法 | |
CN104020480B (zh) | 一种带自适应因子的交互式多模型ukf的卫星导航方法 | |
CN103063212B (zh) | 一种基于非线性映射自适应混合Kalman/H∞滤波器的组合导航方法 | |
CN106291645A (zh) | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 | |
CN104331623B (zh) | 一种机动策略自适应的目标跟踪信息滤波方法 | |
CN110503071A (zh) | 基于变分贝叶斯标签多伯努利叠加模型的多目标跟踪方法 | |
CN107862134A (zh) | 一种考虑自相关测量误差的Wiener过程可靠性分析方法 | |
CN106885576A (zh) | 一种基于多点地形匹配定位的auv航迹偏差估计方法 | |
CN107290742B (zh) | 一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法 | |
CN103973263B (zh) | 一种逼近滤波方法 | |
CN101853243A (zh) | 系统模型未知的自适应卡尔曼滤波方法 | |
CN105929340A (zh) | 一种基于arima估算电池soc的方法 | |
CN106525042A (zh) | 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法 | |
CN107169478A (zh) | 一种自适应在线滤波方法 | |
CN104868876B (zh) | 一种针对过程噪声协方差矩阵Q未知情况下的Kalman滤波方法 | |
CN103218482B (zh) | 一种动力学系统中不确定参数的估计方法 | |
CN103237320B (zh) | 无线传感器网络基于混合量化卡尔曼融合的目标跟踪方法 | |
CN106200377A (zh) | 一种飞行器控制参数的估计方法 | |
CN104021285B (zh) | 一种具有最优运动模式切换参数的交互式多模型目标跟踪方法 | |
CN105043384A (zh) | 一种基于鲁棒Kalman滤波的陀螺随机噪声ARMA模型建模方法 | |
CN108281961B (zh) | 一种自适应鲁棒扩展卡尔曼的参数辨识方法 | |
CN103500455A (zh) | 一种基于无偏有限冲击响应滤波器(ufir)的改进机动目标跟踪方法 | |
CN105701292B (zh) | 一种机动目标转弯角速度的解析辨识方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
EXSB | Decision made by sipo to initiate substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |