CN107632959A - A kind of multi-model self calibration kalman filter method - Google Patents

A kind of multi-model self calibration kalman filter method Download PDF

Info

Publication number
CN107632959A
CN107632959A CN201710832541.4A CN201710832541A CN107632959A CN 107632959 A CN107632959 A CN 107632959A CN 201710832541 A CN201710832541 A CN 201710832541A CN 107632959 A CN107632959 A CN 107632959A
Authority
CN
China
Prior art keywords
mrow
msub
msubsup
mover
msup
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
CN201710832541.4A
Other languages
Chinese (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.)
Beihang University
Original Assignee
Beihang 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 Beihang University filed Critical Beihang University
Priority to CN201710832541.4A priority Critical patent/CN107632959A/en
Publication of CN107632959A publication Critical patent/CN107632959A/en
Pending legal-status Critical Current

Links

Landscapes

  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
  • Nitrogen And Oxygen Or Sulfur-Condensed Heterocyclic Ring Systems (AREA)
  • Medicines Containing Antibodies Or Antigens For Use As Internal Diagnostic Agents (AREA)

Abstract

本发明提供一种多模型自校准卡尔曼滤波方法,步骤如下:一:建立系统基本方程;二:对由式(1)、式(2)和式(3)所组成的系统进行滤波初始化;三:对系统进行时间更新;四:迭代变量更新;五:量测更新;六:进行迭代计算;本发明将多模型估计理论引入系统状态方程受未知输入干扰的问题中,基于自校准卡尔曼滤波和标准卡尔曼滤波,得到了多模型自校准卡尔曼滤波方法的完整过程;既解决了标准卡尔曼滤波在未知输入非零段滤波发散的问题,也显著提升了自校准卡尔曼滤波在未知输入为零段的滤波精度;该发明同时提升了未知输入为零段和非零段的滤波精度,适用范围进一步扩展,系统鲁棒性也在自校准卡尔曼滤波方法的基础上得到了进一步的提升。

The present invention provides a multi-model self-calibration Kalman filter method, the steps are as follows: one: establish the basic equation of the system; two: perform filter initialization to the system composed of formula (1), formula (2) and formula (3); Three: time update the system; four: iterative variable update; five: measurement update; six: iterative calculation; the present invention introduces multi-model estimation theory into the problem that the system state equation is disturbed by unknown input, based on self-calibration Kalman Filtering and standard Kalman filtering, the complete process of the multi-model self-calibrating Kalman filtering method is obtained; it not only solves the problem of standard Kalman filtering divergence in unknown input non-zero segment filtering, but also significantly improves the self-calibrating Kalman filtering in unknown The filtering accuracy of the input is zero segment; the invention improves the filtering accuracy of the unknown input of zero segment and non-zero segment at the same time, the scope of application is further expanded, and the system robustness is further improved on the basis of the self-calibration Kalman filter method promote.

Description

一种多模型自校准卡尔曼滤波方法A multi-model self-calibration Kalman filter method

【技术领域】【Technical field】

本发明提供一种多模型自校准卡尔曼滤波方法,属于鲁棒卡尔曼滤波技术领域。The invention provides a multi-model self-calibration Kalman filtering method, which belongs to the technical field of robust Kalman filtering.

【背景技术】【Background technique】

卡尔曼滤波是一种利用系统状态方程和量测方程对系统状态进行估计的方法,自1960年被提出后已广泛地应用于信号处理、导航、深空探测和故障诊断等领域。标准的卡尔曼滤波方法只针对线性系统,在此基础上,研究人员先后提出了扩展卡尔曼滤波(ExtendedKalman Filter,EKF)、无迹卡尔曼滤波(Unscented Kalman Filter,UKF)和粒子滤波(Particle Filter,PF)等一系列方法,它们将卡尔曼滤波方法的适用范围扩展到了非线性系统,并进一步提高了滤波精度。Kalman filtering is a method of estimating the system state by using the system state equation and measurement equation. Since it was proposed in 1960, it has been widely used in signal processing, navigation, deep space exploration and fault diagnosis and other fields. The standard Kalman filter method is only for linear systems. On this basis, researchers have proposed Extended Kalman Filter (Extended Kalman Filter, EKF), Unscented Kalman Filter (Unscented Kalman Filter, UKF) and Particle Filter (Particle Filter). ,PF) and a series of methods, they extend the scope of application of the Kalman filtering method to nonlinear systems, and further improve the filtering accuracy.

然而上述滤波方法无论是针对线性系统还是非线性系统,都要求系统方程是精确的。但在工程实际中,由于环境因素的影响、模型和参数选取不当等原因,系统状态方程往往受到未知输入的干扰,从而使滤波精度下降,甚至导致滤波发散。针对此问题,文献“自校准Kalman滤波方法[J].航空动力学报.2014,29(06):1363-1368”提出了一种自校准卡尔曼滤波方法(Self-calibration Kalman Filter,SKF),该方法在依照原始状态方程进行迭代运算的同时,对未知输入项进行估计,从而使未知输入的影响自动得到补偿。However, whether the above filtering methods are aimed at linear systems or nonlinear systems, the system equations are required to be accurate. However, in engineering practice, due to the influence of environmental factors, improper selection of models and parameters, etc., the system state equation is often disturbed by unknown inputs, which reduces the filtering accuracy and even leads to filtering divergence. In response to this problem, the document "Self-calibration Kalman filter method [J]. Aerodynamics Journal. 2014, 29 (06): 1363-1368" proposed a self-calibration Kalman filter method (Self-calibration Kalman Filter, SKF), The method estimates the unknown input items while performing iterative operations according to the original state equation, so that the influence of the unknown input is automatically compensated.

但是由于系统的不确定性,系统状态方程的未知输入也可能为零。在这种情况下,自校准卡尔曼滤波方法由于引入了未知输入估计项,虽然这一项会随着滤波的进行逐渐收敛至零,但考虑到收敛过程的波动性、时延和误差的存在,其滤波精度就不及标准卡尔曼滤波方法。因此,在保持未知输入非零段滤波精度的同时,提升未知输入为零段的精度是自校准卡尔曼滤波方法进一步完善的方向。But due to the uncertainty of the system, the unknown input of the system state equation may also be zero. In this case, since the self-calibration Kalman filter method introduces an unknown input estimation term, although this term will gradually converge to zero as the filtering progresses, considering the volatility, time delay and error of the convergence process , its filtering accuracy is not as good as the standard Kalman filtering method. Therefore, while maintaining the accuracy of the unknown input non-zero segment filter, improving the accuracy of the unknown input to the zero segment is the direction for further improvement of the self-calibration Kalman filter method.

多模型估计理论(Multiple-Model Estimation,MME)为这一问题的解决提供了有效途径。其基本原理是:由于人为建模不可能完全精确,则针对一个系统同时建立多个动力学模型进行描述,所有模型均参与运算并得到多个一步预测值和一步预测误差方差矩阵,再根据贝叶斯定理迭代计算出在量测值已知情况下各动力学模型对应的条件概率,通过加权融合进而得到最终的状态估计值。Multiple-Model Estimation (MME) provides an effective way to solve this problem. The basic principle is: since artificial modeling cannot be completely accurate, multiple dynamic models are established to describe a system at the same time. Yees' theorem iteratively calculates the conditional probability corresponding to each dynamic model when the measured value is known, and then obtains the final state estimation value through weighted fusion.

【发明内容】【Content of invention】

本发明的目的是提供一种多模型自校准卡尔曼滤波方法(Multiple-model Self-calibration Kalman Filter,MSKF),它采用自校准卡尔曼滤波与标准卡尔曼滤波同时进行迭代运算,并在每一时刻得到两种方法滤波结果对应的权重,通过加权融合发挥它们在不同阶段各自的优势。大量仿真结果表明,无论系统是否受到未知输入的影响,本发明都能够确保良好的滤波结果。The purpose of the present invention is to provide a multi-model self-calibration Kalman filter method (Multiple-model Self-calibration Kalman Filter, MSKF), it uses self-calibration Kalman filter and standard Kalman filter to carry out iterative operation simultaneously, and in each The weights corresponding to the filtering results of the two methods are obtained at all times, and their respective advantages at different stages are brought into play through weighted fusion. A large number of simulation results show that the invention can ensure good filtering results no matter whether the system is affected by unknown input or not.

本发明一种多模型自校准卡尔曼滤波方法,它包含以下六个步骤:A kind of multi-model self-calibration Kalman filter method of the present invention, it comprises following six steps:

步骤一:建立系统基本方程Step 1: Establish the basic equations of the system

多模型自校准卡尔曼滤波采用自校准卡尔曼滤波与标准卡尔曼滤波两种方法进行运算,故系统包含两个状态方程,第一个为自校准状态方程,第二个为标准的状态方程,其具体表达式为The multi-model self-calibration Kalman filter adopts two methods of self-calibration Kalman filter and standard Kalman filter, so the system contains two state equations, the first is the self-calibration state equation, and the second is the standard state equation, Its specific expression is

Zk=HkXk+Vk (3)Z k =H k X k +V k (3)

式中,Xk表示系统的状态向量,分别对应含未知输入的动力学模型和标准的动力学模型,Zk表示系统量测向量,Φk和Hk分别为状态转移矩阵和量测矩阵,bk表示未知输入,Wk与Vk分别为系统噪声向量和量测噪声向量,其方差矩阵分别为Qk和Rk,并且满足where X k represents the state vector of the system, with Corresponding to the dynamic model with unknown input and the standard dynamic model, Z k represents the system measurement vector, Φ k and H k are the state transition matrix and measurement matrix, respectively, b k represents the unknown input, W k and V k are the system noise vector and the measurement noise vector respectively, and their variance matrices are Q k and R k respectively, and satisfy

式中,Cov[·]为协方差,E[·]为数学期望,δkj为δ函数,当k=j时,δkj=1,当k≠j时,δkj=0;In the formula, Cov[ ] is covariance, E[ ] is mathematical expectation, δ kj is δ function, when k=j, δ kj =1, when k≠j, δ kj =0;

步骤二:对由式(1)、式(2)和式(3)所组成的系统进行滤波初始化Step 2: Perform filter initialization on the system composed of formula (1), formula (2) and formula (3)

设定状态估计与估计误差方差矩阵的初始值为Set the initial value of the state estimation and estimation error variance matrix to be

同时,为了完成两模型估计结果的融合,还需要设定两种模型的概率初始值At the same time, in order to complete the fusion of the estimated results of the two models, it is also necessary to set the initial value of the probability of the two models

Pr(1|Z3)=Pr(2|Z3)=0.5 (7)Pr(1|Z 3 )=Pr(2|Z 3 )=0.5 (7)

以及用于迭代计算的概率初始值Prmax和Prmin;初始化Prmax和Prmin的原因如下:And the probability initial values Pr max and Pr min for iterative calculation; the reasons for initializing Pr max and Pr min are as follows:

在多模型估计运算过程中,一些模型会由于对应的概率逐渐趋近为零而被淘汰,故参与运算的模型数量N在不断减小,这会降低系统对复杂环境的适应能力;目前国际上针对这一问题的解决途径是为系统设置概率下限,当某一模型的概率值低于此下限时,概率值便保持此下限值不再改变;这样处理的优点是在不影响估计结果的同时保持了模型的多样性,但缺点同样明显,即处于概率下限的模型如果要再次发挥作用,其概率值需要较长时间的迭代运算才能恢复,这对卡尔曼滤波的实时性有很大的影响;为了克服这一缺点,针对本文方法只选取两个动力学模型且采用最高概率法选取先验估计值,故只需要定性分析两种模型的概率大小而不需要精确计算概率值的特点,多模型自校准卡尔曼滤波方法不再使用计算得到的条件概率值进行迭代,而是设定两个确定的概率初始值Prmin和Prmax=1-Prmin;在每一步滤波之前,通过比较上一步概率结果的大小将其分别赋给两个模型,并以它们为初始值更新当前时刻的模型概率;由于Prmin并不是概率下限那样的极小值,因此可以保证概率恢复的速度,从而使卡尔曼滤波的实时性得到保证;In the process of multi-model estimation calculation, some models will be eliminated because the corresponding probability gradually approaches zero, so the number N of models participating in the calculation is constantly decreasing, which will reduce the adaptability of the system to complex environments; The solution to this problem is to set a probability lower limit for the system. When the probability value of a certain model is lower than this lower limit, the probability value will remain at this lower limit and will not change; the advantage of this treatment is that it does not affect the estimation results. At the same time, the diversity of the model is maintained, but the disadvantage is also obvious, that is, if the model at the lower limit of the probability is to function again, its probability value needs a long time of iterative operation to recover, which has a great impact on the real-time performance of Kalman filtering. influence; in order to overcome this shortcoming, only two dynamic models are selected for the method of this paper and the prior estimation value is selected by the highest probability method, so it is only necessary to qualitatively analyze the probability of the two models without accurately calculating the characteristics of the probability value. The multi-model self-calibration Kalman filter method no longer uses the calculated conditional probability value to iterate, but sets two determined initial probability values Pr min and Pr max =1-Pr min ; before each step of filtering, by comparing The size of the probability result in the previous step is assigned to the two models respectively, and the model probability at the current moment is updated with them as initial values; since Pr min is not a minimum value like the lower limit of the probability, the speed of probability recovery can be guaranteed, so that The real-time performance of Kalman filtering is guaranteed;

步骤三:对系统进行时间更新Step 3: Update the system time

当k=1,2时,状态一步预测值When k=1,2, state one-step forecast value

一步预测误差方差矩阵One-step forecast error variance matrix

P1/0=Φ0P0Φ0 T+Q0 (10)P 1/0 =Φ 0 P 0 Φ 0 T +Q 0 (10)

P2/1=Φ1P1Φ1 T+Q1 (11)P 2/1 =Φ 1 P 1 Φ 1 T +Q 1 (11)

当k>2时,状态一步预测值When k>2, state one-step forecast value

一步预测误差方差矩阵One-step forecast error variance matrix

式中In the formula

J=argmaxjPr(j|Zk) (14)J=argmax j Pr(j|Z k ) (14)

其中,函数argmax[f(x)]返回当f(x)最大时x的值,并且where the function argmax[f(x)] returns the value of x when f(x) is maximum, and

S1=P1 (22)S 1 =P 1 (22)

在上述计算过程中,式(15)提供自校准状态方程的一步预测结果,式(16)提供标准状态方程计算的一步预测结果,然后通过式(12)中概率大小的比对,由式(14)完成最终的一步预测值筛选;概率大小的计算依托于贝叶斯原理,比较的是在当前时刻量测值已知的情况下,两种模型的条件概率,如式(19)和式(20)所示,其他计算过程则来自于多模型估计理论;其时间更新流程示意图见图2所示;In the above calculation process, the formula (15) provides the one-step prediction result of the self-calibration state equation, and the formula (16) provides the one-step prediction result of the standard state equation calculation, and then through the comparison of the probability in the formula (12), the formula ( 14) Complete the final one-step prediction value screening; the calculation of the probability depends on the Bayesian principle, and the comparison is the conditional probability of the two models when the measured value at the current moment is known, such as formula (19) and formula As shown in (20), other calculation processes come from the multi-model estimation theory; the schematic diagram of the time update process is shown in Figure 2;

步骤四:迭代变量更新Step 4: Iterative variable update

在步骤(三)中有很多中间变量需要实时更新,因此有必要得到它们的递推公式,进而保证整个滤波过程的顺利进行;这些迭代变量包括:In step (3), there are many intermediate variables that need to be updated in real time, so it is necessary to obtain their recursive formulas to ensure the smooth progress of the entire filtering process; these iterative variables include:

各模型量测更新Each model measurement update

各模型条件概率重置Conditional probability reset for each model

Pr(J|Zk)=Prmax (28)Pr(J|Z k )=Pr max (28)

Pr[(3-J)|Zk]=Prmin (29)Pr[(3-J)|Z k ]=Pr min (29)

;

步骤五:量测更新Step 5: Measurement update

滤波增益矩阵filter gain matrix

状态估计值state estimate

此步骤的计算公式由于不涉及自校准技术的基本假设与多模型估计理论的相关内容,所以计算过程与标准卡尔曼滤波方法的后验估计公式保持一致;同时需要说明的是,由于多模型自校准卡尔曼滤波方法是通过两个模型结果的融合计算得到的,因此在计算过程中并不需要估计误差方差矩阵在相邻时刻间的传递,所以在计算步骤中没有给出Pk的计算公式;Since the calculation formula of this step does not involve the basic assumptions of the self-calibration technology and the relevant content of the multi-model estimation theory, the calculation process is consistent with the posterior estimation formula of the standard Kalman filtering method; The calibration Kalman filter method is calculated by the fusion of two model results, so it is not necessary to estimate the transfer of the error variance matrix between adjacent moments in the calculation process, so the calculation formula of Pk is not given in the calculation step ;

步骤六:迭代计算Step 6: Iterative calculation

根据k时刻的两个模型分别得到的状态估计值和误差方差矩阵Pk,重复步骤(三)、(四)和(五),进而得到k+1时刻的状态估计值,往复迭代,直至滤波过程结束。State estimates obtained from the two models at time k and the error variance matrix P k , repeat steps (3), (4) and (5), and then obtain the estimated value of the state at k+1 time, and iterate back and forth until the filtering process ends.

综上所述:步骤(一)建立了多模型估计理论所需的两个动力学模型(状态方程),为后续的估计结果融合提供了先决条件;步骤(二)保证了滤波方法的正常启动;步骤(三)作为本发明的核心步骤,完成了两个模型估计结果的融合,使本发明得以适应不同的复杂环境;步骤(四)则是步骤(三)得以顺利进行的重要保证,通过其对中间变量的更新,步骤(三)才能够在每一个时刻都能够正常使用;步骤(五)通过引入量测信息,提供了状态的后验估计,使滤波精度进一步提升;步骤(六)保障了滤波的正常进行,使其可以运行至实际需要的结束时间;也就是说,本发明将多模型估计理论引入系统状态方程受未知输入干扰的问题中,基于自校准卡尔曼滤波和标准卡尔曼滤波,在理论上推导得到了多模型自校准卡尔曼滤波方法的完整过程;该发明既解决了标准卡尔曼滤波在未知输入非零段滤波发散的问题,也显著提升了自校准卡尔曼滤波在未知输入为零段的滤波精度;该发明同时提升了未知输入为零段和非零段的滤波精度,适用范围进一步扩展,系统鲁棒性也在自校准卡尔曼滤波方法的基础上得到了进一步的提升。In summary: step (1) establishes two dynamic models (state equations) required by the multi-model estimation theory, which provides prerequisites for the subsequent fusion of estimation results; step (2) ensures the normal startup of the filtering method Step (3) as the core step of the present invention has completed the fusion of two model estimation results, so that the present invention can adapt to different complex environments; Step (4) is an important guarantee that step (3) can be carried out smoothly. For the update of intermediate variables, step (3) can be used normally at every moment; step (5) provides a posteriori estimation of the state by introducing measurement information, which further improves the filtering accuracy; step (6) The normal operation of the filtering is guaranteed, so that it can run to the actual required end time; that is to say, the present invention introduces the multi-model estimation theory into the problem that the system state equation is disturbed by unknown input, based on self-calibration Kalman filter and standard Karl Mann filter, the complete process of the multi-model self-calibration Kalman filter method is derived theoretically; this invention not only solves the problem of standard Kalman filter divergence in unknown input non-zero segment filtering, but also significantly improves the self-calibration Kalman filter The filtering accuracy when the unknown input is zero segment; the invention improves the filtering accuracy of the unknown input being zero segment and non-zero segment at the same time, the scope of application is further expanded, and the system robustness is also improved on the basis of the self-calibration Kalman filter method further improvement.

本发明的优点与积极效果在于:Advantage and positive effect of the present invention are:

(1)将多模型估计理论引入系统状态方程受未知输入干扰的问题中,基于自校准卡尔曼滤波和标准卡尔曼滤波,在理论上推导得到了多模型自校准卡尔曼滤波方法的完整过程。(1) The multi-model estimation theory is introduced into the problem that the system state equation is disturbed by unknown input. Based on the self-calibration Kalman filter and the standard Kalman filter, the complete process of the multi-model self-calibration Kalman filter method is theoretically derived.

(2)该发明融合了各种方法的优点,既解决了标准卡尔曼滤波在未知输入非零段滤波发散的问题,也显著提升了自校准卡尔曼滤波在未知输入为零段的滤波精度。(2) The invention combines the advantages of various methods, which not only solves the problem of standard Kalman filter divergence when the unknown input is non-zero, but also significantly improves the filtering accuracy of the self-calibration Kalman filter when the unknown input is zero.

(3)该发明同时提升了未知输入为零段和非零段的滤波精度,适用范围进一步扩展,系统鲁棒性也在自校准卡尔曼滤波方法的基础上得到了进一步的提升。(3) The invention simultaneously improves the filtering accuracy of the unknown input being zero segment and non-zero segment, the scope of application is further expanded, and the robustness of the system is further improved on the basis of the self-calibration Kalman filter method.

【附图说明】【Description of drawings】

图1为本发明方法流程示意图。Fig. 1 is a schematic flow chart of the method of the present invention.

图2为步骤(三)时间更新流程示意图。FIG. 2 is a schematic diagram of the time updating process in step (3).

发明中序号、符号、代号说明如下:The serial numbers, symbols and codes in the invention are explained as follows:

Xk:系统的状态向量X k : the state vector of the system

含未知输入的动力学模型的状态向量 State vector for a kinetic model with unknown inputs

标准动力学模型的状态向量 The state vector of the standard kinetic model

Zk:系统量测向量Z k : system measurement vector

Φk:状态转移矩阵Φ k : state transition matrix

Hk:量测矩阵H k : measurement matrix

bk:未知输入b k : unknown input

Wk:系统噪声向量W k : System noise vector

Vk:量测噪声向量V k : measurement noise vector

Qk:系统噪声向量方差矩阵Q k : System noise vector variance matrix

Rk:量测噪声向量方差矩阵R k : measurement noise vector variance matrix

Cov[·]:协方差Cov[ ]: covariance

E[·]:数学期望E[·]: Mathematical Expectation

δkj:δ函数δ kj : δ function

Pr(1|Zk)、Pr(2|Zk):迭代计算的概率初始值Pr(1|Z k ), Pr(2|Z k ): the initial value of the probability of iterative calculation

Prmax、Prmin:用于迭代计算的概率初始值Pr max , Pr min : the initial value of the probability used for iterative calculation

Pk/k-1:一步预测误差方差矩阵P k/k-1 : one-step forecast error variance matrix

argmax[f(x)]:函数最大值返回函数argmax[f(x)]: function maximum return function

rk:残差r k : residual

Tk:量测估计误差方差矩阵T k : measurement estimation error variance matrix

Kk:滤波增益矩阵K k : filter gain matrix

状态估计 state estimation

【具体实施方式】【detailed description】

下面结合附图对本发明作详细说明。The present invention will be described in detail below in conjunction with the accompanying drawings.

本发明提出了一种多模型自校准卡尔曼滤波方法,其流程图如图1所示,时间更新流程图如图2所示,它包括以下六个步骤:The present invention proposes a kind of multi-model self-calibration Kalman filter method, its flow chart as shown in Figure 1, time updating flow chart as shown in Figure 2, it comprises following six steps:

步骤一:建立系统基本方程Step 1: Establish the basic equations of the system

Zk=HkXk+Vk (34)Z k =H k X k +V k (34)

式中,Xk表示系统的状态向量,分别对应含未知输入的动力学模型和标准的动力学模型,Zk表示系统量测向量,Φk和Hk分别为状态转移矩阵和量测矩阵,bk表示未知输入,Wk与Vk分别为系统噪声向量和量测噪声向量,其方差矩阵分别为Qk和Rk,并且满足where X k represents the state vector of the system, with Corresponding to the dynamic model with unknown input and the standard dynamic model, Z k represents the system measurement vector, Φ k and H k are the state transition matrix and measurement matrix, respectively, b k represents the unknown input, W k and V k are the system noise vector and the measurement noise vector respectively, and their variance matrices are Q k and R k respectively, and satisfy

式中,Cov[·]为协方差,E[·]为数学期望,δkj为δ函数,当k=j时,δkj=1,当k≠j时,δkj=0。In the formula, Cov[ ] is covariance, E[ ] is mathematical expectation, δ kj is δ function, when k=j, δ kj =1, when k≠j, δ kj =0.

步骤二:滤波初始化Step 2: Filter initialization

设定状态估计与估计误差方差矩阵的初始值为Set the initial value of the state estimation and estimation error variance matrix to be

同时,设定两种模型的概率初始值At the same time, set the initial value of the probability of the two models

Pr(1|Z3)=Pr(2|Z3)=0.5 (38)Pr(1|Z 3 )=Pr(2|Z 3 )=0.5 (38)

以及用于迭代计算的概率初始值Prmax和PrminAnd the probability initial values Pr max and Pr min for iterative calculation.

步骤三:时间更新Step 3: Time update

当k=1,2时,状态一步预测值When k=1,2, state one-step forecast value

一步预测误差方差矩阵One-step forecast error variance matrix

P1/0=Φ0P0Φ0 T+Q0 (41)P 1/0 =Φ 0 P 0 Φ 0 T +Q 0 (41)

P2/1=Φ1P1Φ1 T+Q1 (42)P 2/1 =Φ 1 P 1 Φ 1 T +Q 1 (42)

当k>2时,状态一步预测值When k>2, state one-step forecast value

一步预测误差方差矩阵One-step forecast error variance matrix

式中In the formula

J=argmaxjPr(j|Zk) (45)J=argmax j Pr(j|Z k ) (45)

其中,函数argmax[f(x)]返回当f(x)最大时x的值,并且where the function argmax[f(x)] returns the value of x when f(x) is maximum, and

S1=P1 (53)S 1 =P 1 (53)

其时间更新流程示意图见图2所示;The schematic diagram of the time update process is shown in Figure 2;

步骤四:迭代变量更新Step 4: Iterative variable update

各模型量测更新Each model measurement update

各模型条件概率重置Conditional probability reset for each model

Pr(J|Zk)=Prmax (59)Pr(J|Z k )=Pr max (59)

Pr[(3-J)|Zk]=Prmin (60)Pr[(3-J)|Z k ]=Pr min (60)

;

步骤五:量测更新Step 5: Measurement update

滤波增益矩阵filter gain matrix

状态估计值state estimate

;

步骤六:迭代计算Step 6: Iterative calculation

k=k+1 (65)k=k+1 (65)

重复步骤三、步骤四和步骤五,直至滤波过程结束。Repeat steps 3, 4 and 5 until the filtering process ends.

Claims (1)

  1. A kind of 1. multi-model self calibration kalman filter method, it is characterised in that:It includes following six step:
    Step 1:Establish system fundamental equation
    Multi-model self calibration Kalman filtering is transported using two methods of self calibration Kalman filtering and standard Kalman filtering Calculate, therefore system includes two state equations, first is self calibration state equation, and second state equation for standard, it has Body expression formula is
    <mrow> <msubsup> <mi>X</mi> <mi>k</mi> <mn>1</mn> </msubsup> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <mo>+</mo> <msub> <mi>b</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>W</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msubsup> <mi>X</mi> <mi>k</mi> <mn>2</mn> </msubsup> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>+</mo> <msub> <mi>W</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
    Zk=HkXk+Vk·············(3)
    In formula, XkThe state vector of expression system,WithCorresponding kinetic model and standard containing Unknown worm is dynamic respectively Mechanical model, ZkRepresent system measurements vector, ΦkAnd HkRespectively state-transition matrix and measurement matrix, bkRepresent unknown defeated Enter, WkWith VkRespectively system noise vector sum measures noise vector, and its variance matrix is respectively QkAnd Rk, and meet
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>C</mi> <mi>o</mi> <mi>v</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>W</mi> <mi>j</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <msubsup> <mi>W</mi> <mi>j</mi> <mi>T</mi> </msubsup> <mo>&amp;rsqb;</mo> <mo>=</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <msub> <mi>&amp;delta;</mi> <mrow> <mi>k</mi> <mi>j</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>V</mi> <mi>k</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>C</mi> <mi>o</mi> <mi>v</mi> <mo>&amp;lsqb;</mo> <msub> <mi>V</mi> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>V</mi> <mi>j</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>V</mi> <mi>k</mi> </msub> <msubsup> <mi>V</mi> <mi>j</mi> <mi>T</mi> </msubsup> <mo>&amp;rsqb;</mo> <mo>=</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <msub> <mi>&amp;delta;</mi> <mrow> <mi>k</mi> <mi>j</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>C</mi> <mi>o</mi> <mi>v</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <mo>,</mo> <msub> <mi>V</mi> <mi>j</mi> </msub> <mo>&amp;rsqb;</mo> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <msubsup> <mi>V</mi> <mi>j</mi> <mi>T</mi> </msubsup> <mo>&amp;rsqb;</mo> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mn>...</mn> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
    In formula, Cov [] is covariance, and E [] is mathematic expectaion, δkjFor δ functions, as k=j, δkj=1, as k ≠ j, δkj=0;
    Step 2:Initialization is filtered to the system being made up of formula (1), formula (2) and formula (3)
    Set the initial value of state estimation and estimation error variance matrix as
    <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <msub> <mi>X</mi> <mn>0</mn> </msub> <mo>&amp;rsqb;</mo> <mn>...</mn> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>P</mi> <mn>0</mn> </msub> <mo>=</mo> <mi>E</mi> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> <mn>...</mn> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
    Meanwhile in order to complete the fusion of two model estimated results, it is also necessary to set the probability initial value of two kinds of models
    Pr(1|Z3)=Pr (2 | Z3)=0.5 (7)
    And the probability initial value Pr for iterative calculationmaxAnd Prmin;Initialize PrmaxAnd PrminThe reason for it is as follows:
    In multiple-model estimator calculating process, some models can be eliminated due to the gradual convergence of corresponding probability for zero, therefore be joined Model quantity N with computing is constantly reducing, and this can reduce adaptability of the system to complex environment;Only chosen for the present invention Two kinetic models and use maximum probability method selection priori estimates, therefore only need the probability of two kinds of models of qualitative analysis big It is small without accurate the characteristics of calculating probable value, multi-model self calibration kalman filter method does not use the bar being calculated Part probable value is iterated, but sets the probability initial value Pr of two determinationsminAnd Prmax=1-Prmin;Filtered in each step Before, it is assigned to two models respectively by comparing the size of previous step probability results, and worked as using them as initial value renewal The model probability at preceding moment;Due to PrminIt is not minimum as probability lower limit, therefore the speed that guarantee probability recovers, from And the real-time of Kalman filtering is set to be guaranteed;
    Step 3:Time renewal is carried out to system
    Work as k=1, when 2, state one-step prediction value
    <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mn>1</mn> <mo>/</mo> <mn>0</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mn>0</mn> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mn>2</mn> <mo>/</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mn>1</mn> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
    One-step prediction varivance matrix
    <mrow> <msub> <mi>P</mi> <mrow> <mn>1</mn> <mo>/</mo> <mn>0</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mn>0</mn> </msub> <msub> <mi>P</mi> <mn>0</mn> </msub> <msubsup> <mi>&amp;Phi;</mi> <mn>0</mn> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>Q</mi> <mn>0</mn> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
    P2/11P1Φ1 T+Q1············(11)
    As k > 2, state one-step prediction value
    <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>J</mi> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
    One-step prediction varivance matrix
    <mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>J</mi> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
    In formula
    J=argmaxjPr(j|Zk)···········(14)
    <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <mo>-</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <msubsup> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;times;</mo> <msubsup> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <msubsup> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;times;</mo> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>=</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <msup> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mi>T</mi> </msup> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
    Wherein, function argmax [f (x)] returns to the value of the x when f (x) is maximum, and
    <mrow> <mi>Pr</mi> <mrow> <mo>(</mo> <mi>j</mi> <mo>|</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mrow> <mi>p</mi> <mi>d</mi> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>|</mo> <mi>j</mi> <mo>)</mo> </mrow> <mi>Pr</mi> <mrow> <mo>(</mo> <mi>j</mi> <mo>|</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mi>p</mi> <mi>d</mi> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>|</mo> <mi>i</mi> <mo>)</mo> </mrow> <mi>Pr</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>|</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mfrac> <mn>...</mn> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <mi>p</mi> <mi>d</mi> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>|</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>&amp;ap;</mo> <mfrac> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <mo>-</mo> <msubsup> <mi>r</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msubsup> <mi>T</mi> <mi>k</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <msub> <mi>r</mi> <mi>k</mi> </msub> <mo>/</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msup> <mrow> <mo>(</mo> <mn>2</mn> <mi>&amp;pi;</mi> <mo>)</mo> </mrow> <mrow> <mi>q</mi> <mo>/</mo> <mn>2</mn> </mrow> </msup> <mo>|</mo> <msub> <mi>T</mi> <mi>k</mi> </msub> <msup> <mo>|</mo> <mrow> <mn>1</mn> <mo>/</mo> <mn>2</mn> </mrow> </msup> </mrow> </mfrac> <mn>...</mn> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mo>&amp;lsqb;</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>+</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>&amp;Phi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>3</mn> </mrow> </msub> <msubsup> <mi>S</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>3</mn> </mrow> </msub> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> <mn>....</mn> <mrow> <mo>(</mo> <mn>21</mn> <mo>)</mo> </mrow> </mrow>
    S1=P1··············(22)
    <mrow> <msub> <mi>r</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>23</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>T</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>24</mn> <mo>)</mo> </mrow> </mrow>
    In above-mentioned calculating process, formula (15) provides the one-step prediction result of self calibration state equation, and formula (16) provides standard shape The one-step prediction result that state equation calculates, then by the comparison of probability size in formula (12), final one is completed by formula (14) Walk predicted value screening;The conditional probability of two kinds of models, as shown in formula (19) and formula (20);
    Step 4:Iteration variable updates
    There are many intermediate variables to need real-time update in step (3), it is therefore necessary to their recurrence formula is obtained, and then Ensure being smoothed out for whole filtering;These iteration variables include:
    Each model measurement renewal
    <mrow> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mn>...</mn> <mrow> <mo>(</mo> <mn>25</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>+</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mn>...</mn> <mrow> <mo>(</mo> <mn>26</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msubsup> <mi>P</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>j</mi> </msubsup> <msup> <mrow> <mo>(</mo> <mi>I</mi> <mo>-</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <msub> <mi>R</mi> <mi>k</mi> </msub> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mn>......</mn> <mrow> <mo>(</mo> <mn>27</mn> <mo>)</mo> </mrow> </mrow>
    Each Model Condition probability is reset
    Pr(J|Zk)=Prmax············(28)
    Pr[(3-J)|Zk]=Prmin···········(29)
    Step 5:Measure renewal
    Filtering gain matrix
    <mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mn>...</mn> <mrow> <mo>(</mo> <mn>30</mn> <mo>)</mo> </mrow> </mrow>
    State estimation
    <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mn>...</mn> <mrow> <mo>(</mo> <mn>31</mn> <mo>)</mo> </mrow> </mrow>
    The calculation formula of this step due to not being related to the basic assumption of self-calibration technique and the related content of Multiple model estimation theory, So calculating process and the Posterior estimator formula of standard Kalman filtering method are consistent;It should be noted simultaneously that due to Multi-model self calibration kalman filter method is obtained by the fusion calculation of two model results, therefore in calculating process And transmission of the estimation error variance matrix between adjacent moment is not needed, so not providing P in calculation procedurekCalculating it is public Formula;
    Step 6:Iterative calculation
    The state estimation respectively obtained according to the two of the k moment modelsWith varivance matrix Pk, repeat step (three), (4) and (five), and then the state estimation at k+1 moment is obtained, reciprocal iteration, until filtering terminates.
CN201710832541.4A 2017-09-15 2017-09-15 A kind of multi-model self calibration kalman filter method Pending CN107632959A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710832541.4A CN107632959A (en) 2017-09-15 2017-09-15 A kind of multi-model self calibration kalman filter method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710832541.4A CN107632959A (en) 2017-09-15 2017-09-15 A kind of multi-model self calibration kalman filter method

Publications (1)

Publication Number Publication Date
CN107632959A true CN107632959A (en) 2018-01-26

Family

ID=61102249

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710832541.4A Pending CN107632959A (en) 2017-09-15 2017-09-15 A kind of multi-model self calibration kalman filter method

Country Status (1)

Country Link
CN (1) CN107632959A (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108537360A (en) * 2018-03-01 2018-09-14 长安大学 One kind taking polyfactorial adaptable Kalman filter slide prediction method into account
CN110110711A (en) * 2019-06-06 2019-08-09 郑州轻工业学院 A kind of iterative learning control systems input signal estimation method under noisy communication channel
CN117909850A (en) * 2024-03-18 2024-04-19 中铁电气化局集团有限公司 Carrier cable supporting device vibration signal processing method based on fusion algorithm

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108537360A (en) * 2018-03-01 2018-09-14 长安大学 One kind taking polyfactorial adaptable Kalman filter slide prediction method into account
CN110110711A (en) * 2019-06-06 2019-08-09 郑州轻工业学院 A kind of iterative learning control systems input signal estimation method under noisy communication channel
CN110110711B (en) * 2019-06-06 2021-06-04 郑州轻工业学院 Iterative learning control system input signal estimation method under noise channel
CN117909850A (en) * 2024-03-18 2024-04-19 中铁电气化局集团有限公司 Carrier cable supporting device vibration signal processing method based on fusion algorithm
CN117909850B (en) * 2024-03-18 2024-06-04 中铁电气化局集团有限公司 Carrier cable supporting device vibration signal processing method based on fusion algorithm

Similar Documents

Publication Publication Date Title
CN111985093B (en) An Adaptive Unscented Kalman Filter State Estimation Method with Noise Estimator
CN107547067A (en) A kind of multi-model self calibration EKF method
CN107783944A (en) A kind of multi-model self calibration Unscented kalman filtering method
CN106021194B (en) A kind of multi-sensor multi-target tracking bias estimation method
CN106487358A (en) A kind of maximal correlation entropy volume kalman filter method based on statistical linear regression
CN106441300B (en) An Adaptive Cooperative Navigation Filtering Method
CN104462015B (en) Process the fractional order linear discrete system state updating method of non-gaussian L é vy noises
CN107565931A (en) A kind of self calibration Unscented kalman filtering method
CN104318072B (en) QKF-MMF (Quantitative Kalman Filtering-Multi Method Fusion) based multi-sensor quantitative fusion method
CN107632959A (en) A kind of multi-model self calibration kalman filter method
CN104021289A (en) Non-Gaussian unsteady-state noise modeling method
CN108846523A (en) A kind of flight for putting forth coasting time dynamic prediction method based on Bayesian network
CN109341690B (en) A Robust and Efficient Adaptive Data Fusion Method for Integrated Navigation
WO2020052213A1 (en) Iterative cubature unscented kalman filtering method
CN107797106A (en) A kind of PHD multiple target tracking smooth filtering methods of the unknown clutter estimations of acceleration EM
CN104298650B (en) Multi-method fusion based Kalman filtering quantization method
Aravkin et al. The connection between Bayesian estimation of a Gaussian random field and RKHS
CN107807906A (en) A kind of multi-model self calibration order filtering method
CN106909738B (en) A method of model parameter identification
CN108871365B (en) State estimation method and system under course constraint
CN114167295A (en) Lithium ion battery SOC estimation method and system based on multi-algorithm fusion
CN104463214A (en) SVR parameter optimization method based on DRVB-ASCKF
CN105373805A (en) A multi-sensor maneuvering target tracking method based on the principle of maximum entropy
CN109253727A (en) A kind of localization method based on improvement iteration volume particle filter algorithm
CN107886058B (en) Noise-dependent two-stage volumetric Kalman filter estimation method and system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
CB03 Change of inventor or designer information

Inventor after: Yang Haifeng

Inventor after: Fu Huimin

Inventor after: Wang Zhihua

Inventor after: Zhang Yongbo

Inventor before: Fu Huimin

Inventor before: Yang Haifeng

Inventor before: Zhang Yongbo

Inventor before: Wang Zhihua

Inventor before: Xiao Mengli

Inventor before: Cui Die

CB03 Change of inventor or designer information
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20180126

WD01 Invention patent application deemed withdrawn after publication