CN107807906A - A kind of multi-model self calibration order filtering method - Google Patents

A kind of multi-model self calibration order filtering method Download PDF

Info

Publication number
CN107807906A
CN107807906A CN201710853216.6A CN201710853216A CN107807906A CN 107807906 A CN107807906 A CN 107807906A CN 201710853216 A CN201710853216 A CN 201710853216A CN 107807906 A CN107807906 A CN 107807906A
Authority
CN
China
Prior art keywords
mrow
msub
msubsup
mover
mtd
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
CN201710853216.6A
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 CN201710853216.6A priority Critical patent/CN107807906A/en
Publication of CN107807906A publication Critical patent/CN107807906A/en
Pending legal-status Critical Current

Links

Classifications

    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06FELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10Complex mathematical operations
    • G06F17/15Correlation function computation including computation of convolution operations
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06FELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10Complex mathematical operations
    • G06F17/16Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Medicines Containing Antibodies Or Antigens For Use As Internal Diagnostic Agents (AREA)

Abstract

The present invention provides a kind of multi-model self calibration order filtering method, and step is as follows:One:Establish system fundamental equation;Two:Initialization is filtered to system;Three:Time renewal is carried out to system;Four:It is iterated variable update;Five:Carry out measurement renewal;Six:It is iterated calculating;Pass through step 1 to step 6, the present invention takes full advantage of the result of calculation of two methods of order filtering and self calibration order filtering, rely on the Multiple model estimation theory based on Bayes principle, automatic distinguishing Unknown worm is zero section and non-zero section, so as to accurately select wherein most suitable result as the prior estimate of itself;The most important is that the present invention be directed to non-gaussian, nonlinear system to develop, wider compared to the other method scope of application.

Description

一种多模型自校准秩滤波方法A multi-model self-calibration rank filtering method

【技术领域】【Technical field】

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

【背景技术】【Background technique】

针对工程中常见的系统状态方程受未知输入影响的问题,文献“自校准Kalman滤波方法[J].航空动力学报.2014,29(06):1363-1368”提出了一种自校准卡尔曼滤波方法(Self-calibration Kalman Filter,SKF),该方法在依照原始状态方程进行迭代运算的同时,对未知输入项进行估计,从而使未知输入的影响自动得到补偿。在此基础上,研究人员又先后发展了自校准扩展卡尔曼滤波方法(Self-calibration Extended Kalman Filter,SEKF)和自校准无迹卡尔曼滤波方法(Self-calibration Unscented Kalman Filter,SUKF),它们将自校准技术推广到了非线性领域。Aiming at the common problem that the system state equation is affected by unknown input in engineering, 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), this 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. On this basis, researchers have successively developed the Self-calibration Extended Kalman Filter (SEKF) and the Self-calibration Unscented Kalman Filter (SUKF), which will The self-calibration technique is extended to the nonlinear field.

但是由于系统不确定因素的存在,未知输入也有为零的可能。在这种情况下,自校准卡尔曼滤波方法由于在先验估计中引入了对未知输入项的估计,尽管该估计值很小,其滤波精度仍不及没有考虑未知输入影响的标准卡尔曼滤波方法。为了进一步提升自校准卡尔曼滤波在未知输入为零段的滤波精度,研究人员基于多模型估计理论,提出了多模型自校准卡尔曼滤波方法(Multiple-model Self-calibration Kalman Filter,MSKF)。However, due to the existence of system uncertain factors, the unknown input may also be zero. In this case, since the self-calibrating Kalman filter method introduces an estimate of the unknown input item in the prior estimate, although the estimated value is small, its filtering accuracy is still inferior to the standard Kalman filter method that does not consider the influence of the unknown input . In order to further improve the filtering accuracy of the self-calibration Kalman filter when the unknown input is zero, the researchers proposed a multiple-model self-calibration Kalman filter method (Multiple-model Self-calibration Kalman Filter, MSKF) based on the multi-model estimation theory.

特别地,对于非线性系统,多模型自校准扩展卡尔曼滤波(Multiple-model Self-calibration Extended Kalman Filter,MSEKF)和多模型自校准无迹卡尔曼滤波方法(Multiple-model Self-calibration Unscented Kalman Filter,MSUKF)也被开发出来,但是它们只适用于服从高斯分布的系统,对于非高斯分布的非线性系统未知输入存在为零段与非零段的情况便不再适用。In particular, for nonlinear systems, multiple-model self-calibration extended Kalman filter (Multiple-model Self-calibration Extended Kalman Filter, MSEKF) and multi-model self-calibration unscented Kalman filter method (Multiple-model Self-calibration Unscented Kalman Filter , MSUKF) have also been developed, but they are only suitable for systems that obey Gaussian distribution, and it is no longer applicable for nonlinear systems with non-Gaussian distribution where the unknown input exists as zero segment and non-zero segment.

【发明内容】【Content of invention】

本发明的目的是提供一种多模型自校准秩滤波方法(Multiple-model Self-calibration Rank Filter,MSRF),它通过将多模型估计理论引入到自校准秩滤波方法(Self-calibration Rank Filter,SRF)中,将MSKF的应用范围拓展到了非高斯、非线性领域。其同时采用RF与SRF进行计算,实时更新二者权重,进而得到状态估计。The object of the present invention is to provide a kind of multi-model self-calibration rank filter method (Multiple-model Self-calibration Rank Filter, MSRF), it introduces multi-model estimation theory to self-calibration rank filter method (Self-calibration Rank Filter, SRF ), the application range of MSKF is extended to non-Gaussian and nonlinear fields. It uses RF and SRF to calculate at the same time, and updates the weights of the two in real time to obtain state estimation.

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

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

多模型自校准秩滤波采用自校准秩滤波与秩滤波两种方法进行运算,故系统包含两个状态方程,第一个为含有未知输入项的状态方程,第二个为标准的非线性状态方程,其具体表达式为The multi-model self-calibration rank filter adopts two methods of self-calibration rank filter and rank filter to operate, so the system contains two state equations, the first is a state equation with unknown input items, and the second is a standard nonlinear state equation , its concrete expression is

Zk=hk(Xk)+Vk (3)Z k =h k (X k )+V k (3)

式中,Xk表示系统的状态向量,分别对应含未知输入的动力学模型和标准的动力学模型,Zk表示系统量测向量,fk(·)和hk(·)分别为非线性状态递推方程和量测方程,bk表示未知输入,Wk与Vk分别为系统噪声向量和量测噪声向量,其方差矩阵分别为Qk和Rk,并且满足where X k represents the state vector of the system, and Corresponding to the dynamic model with unknown input and the standard dynamic model, Z k represents the system measurement vector, f k ( ) and h k ( ) are the nonlinear state recurrence equation and measurement equation, b k Represents an 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 and initialize the system

设定状态估计与估计误差方差矩阵的初始值为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 operation, some models will be eliminated because the corresponding probability gradually approaches zero, so the number N of models participating in the operation is constantly decreasing, which will reduce the adaptability of the system to complex environments; for the method in this paper Only two dynamic models are selected and the highest probability method is used to select a priori estimated values. It is only necessary to qualitatively analyze the probabilities of the two models without accurately calculating the characteristics of the probability values. The multi-model self-calibration Kalman filter method no longer uses calculations. The obtained conditional probability value is iterated, but two determined probability initial values Pr min and Pr max = 1-Pr min are set; before each step of filtering, they are respectively assigned to the two by comparing the size of the probability result of the previous step. model, and use them as initial values to update the model probability at the current moment; 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时刻的状态估计值和误差方差矩阵分别为和Pk-1,基于它们对系统进行时间更新,即计算k时刻的状态一步预测和一步预测误差方差矩阵Pk/k-1;基于秩滤波的一般性递推公式,在时间更新过程中首先需要计算秩采样点集{χi}Suppose the state estimation value and error variance matrix at time k-1 are respectively and P k-1 , based on them to update the system in time, that is, to calculate the one-step prediction of the state at time k and one-step prediction error variance matrix P k/k-1 ; based on the general recursive formula of rank filtering, it is first necessary to calculate the rank sampling point set {χ i } in the time update process

当k=1,2时when k=1,2

状态一步预测值state one-step predictive value

Xk/k-1,i=f(χk-1,i) (9)X k/k-1,i =f(χ k-1,i ) (9)

式中,n为状态向量Xk的维度,标准正态偏量表示Pk-1平方根的第i列向量;In the formula, n is the dimension of the state vector X k , and the standard normal deviation represents the ith column vector of the square root of P k-1 ;

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

式中,ω为协方差权重系数,其计算公式为In the formula, ω is the covariance weight coefficient, and its calculation formula is

当k>2时When k>2

状态一步预测值state one-step predictive value

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

式中In the formula

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

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

在上述计算过程中,式(18)和式(21)提供自校准状态方程的一步预测结果,式(20)和式(21)提供标准状态方程计算的一步预测结果,然后通过式(15)中概率大小的比对,由式(13)完成最终的一步预测值筛选;概率大小的计算依托于贝叶斯原理,比较的是在当前时刻量测值已知的情况下,两种模型的条件概率,如式(23)和式(24)所示;In the above calculation process, formula (18) and formula (21) provide the one-step prediction result of the self-calibration state equation, and formula (20) and formula (21) provide the one-step prediction result of the standard state equation calculation, and then through the formula (15) For the comparison of the medium probability, the final one-step prediction value screening is completed by formula (13); the calculation of the probability is based on the Bayesian principle, and the comparison is when the measured value at the current moment is known. Conditional probability, as shown in formula (23) and formula (24);

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

在步骤三中有很多中间变量需要实时更新,因此有必要得到它们的递推公式,进而保证整个滤波过程的顺利进行;这些迭代变量包括:In step three, 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

式中In the formula

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

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

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

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

首先,基于状态一步预测和一步预测误差方差矩阵Pk/k-1,根据秩滤波的一般性递推公式对系统状态进行重采样,得到新的秩采样点集{χk/k-1,i}First, one-step prediction based on state and one-step prediction error variance matrix P k/k-1 , resample the system state according to the general recursive formula of rank filtering, and obtain a new rank sampling point set {χ k/k-1,i }

然后,根据{χk/k-1,i}计算量测估计值 Then, calculate the measurement estimate according to {χ k/k-1,i }

Zk/k-1,i=h(χk/k-1,i) (35)Z k/k-1,i =h(χ k/k-1,i ) (35)

由式(35)和式(36)计算量测误差方差矩阵PZZ Calculate the measurement error variance matrix P ZZ by formula (35) and formula (36)

由式(34)、式(35)和式(36)计算误差协方差矩阵PXZ Calculate the error covariance matrix P XZ by formula (34), formula (35) and formula (36)

由式(37)和式(38)计算增益矩阵Calculate the gain matrix by formula (37) and formula (38)

则,由量测更新得到最终的状态估计值和状态估计误差方差矩阵Then, the final state estimation value and state estimation error variance matrix are obtained by the measurement update

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

根据k时刻的状态估计值和误差方差矩阵Pk,重复步骤三、步骤四和步骤五,进而得到k+1时刻的状态估计值和误差方差矩阵,往复迭代,直至滤波过程结束;According to the estimated value of the state 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 and the error variance matrix at k+1, and iterate back and forth until the end of the filtering process;

通过步骤一到步骤六,本发明充分利用了秩滤波和自校准秩滤波两种方法的计算结果,依托基于贝叶斯原理的多模型估计理论,自动区分未知输入为零段与非零段,从而可以精确地选择其中最合适的结果作为自身的先验估计。最重要的一点在于,本发明是针对非高斯、非线性系统开发的,相较于其他方法适用范围更广。Through steps 1 to 6, the present invention makes full use of the calculation results of the two methods of rank filtering and self-calibration rank filtering, and relies on the multi-model estimation theory based on the Bayesian principle to automatically distinguish unknown inputs as zero segments and non-zero segments, Therefore, the most suitable result can be accurately selected as its own prior estimation. The most important point is that the present invention is developed for non-Gaussian, nonlinear systems, and has a wider scope of application than other methods.

本发明的有益效果包括The beneficial effects of the present invention include

(1)将多模型估计理论引入到自校准秩滤波方法中,推导得到了多模型自校准秩滤波方法的完整过程。(1) The multi-model estimation theory is introduced into the self-calibration rank filtering method, and the complete process of the multi-model self-calibration rank filtering method is derived.

(2)可以自动识别未知输入为零段与非零段,分别发挥秩滤波方法与自校准秩滤波方法各自的优势。(2) It can automatically identify the unknown input as zero segment and non-zero segment, and take advantage of the rank filter method and the self-calibration rank filter method respectively.

(3)进一步提高了系统受未知输入影响时的滤波精度,同时提高了滤波过程的稳定性,增强了系统的鲁棒性。(3) The filtering precision when the system is affected by unknown input is further improved, the stability of the filtering process is improved at the same time, and the robustness of the system is enhanced.

(4)可以处理不服从高斯分布的非线性系统,适用范围得以扩展,符合工程应用需求。(4) It can deal with nonlinear systems that do not obey the Gaussian distribution, and the scope of application can be expanded to meet the requirements of engineering applications.

【附图说明】【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 update process in step three.

发明中序号、符号、代号说明如下: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

f(·)、h(·):非高斯、非线性向量函数f( ), h( ): non-Gaussian, nonlinear vector functions

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

i}:秩采样点集i }: rank sampling point set

标准正态偏量 standard normal deviation

Pk-1平方根的第i列向量 i-th column vector of the square root of P k-1

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

ω:协方差权重系数ω: covariance weight coefficient

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

量测估计值 measurement estimate

PZZ:量测误差方差矩阵P ZZ : measurement error variance matrix

PXZ:误差协方差矩阵P XZ : error covariance matrix

状态估计值 state estimate

【具体实施方式】【Detailed ways】

下面结合附图对本发明作详细说明。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 rank 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

式中,Xk表示系统的状态向量,分别对应含未知输入的动力学模型和标准的动力学模型,Zk表示系统量测向量,fk(·)和hk(·)分别为非线性状态递推方程和量测方程,bk表示未知输入,Wk与Vk分别为系统噪声向量和量测噪声向量,其方差矩阵分别为Qk和Rk,并且满足where X k represents the state vector of the system, and Corresponding to the dynamic model with unknown input and the standard dynamic model, Z k represents the system measurement vector, f k ( ) and h k ( ) are the nonlinear state recurrence equation and measurement equation, b k Represents an 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

设定两种模型的概率初始值Set the initial value of the probability of the two models

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

以及用于迭代计算的概率初始值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 predictive value

Xk/k-1,i=f(χk-1,i) (49)X k/k-1,i =f(χ k-1,i ) (49)

式中,n为状态向量Xk的维度,标准正态偏量表示Pk-1平方根的第i列向量。In the formula, n is the dimension of the state vector X k , and the standard normal deviation The ith column vector representing the square root of P k-1 .

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

式中,ω为协方差权重系数,其计算公式为In the formula, ω is the covariance weight coefficient, and its calculation formula is

当k>2时When k>2

状态一步预测值state one-step predictive value

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

式中In the formula

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

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

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

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

各模型量测更新Each model measurement update

式中In the formula

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

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

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

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

进行重采样,得到新的秩采样点集{χk/k-1,i}Perform resampling to get a new rank sampling point set {χ k/k-1,i }

然后,根据{χk/k-1,i}计算量测估计值 Then, calculate the measurement estimate according to {χ k/k-1,i }

Zk/k-1,i=h(χk/k-1,i) (75)Z k/k-1,i =h(χ k/k-1,i ) (75)

计算量测误差方差矩阵PZZ Calculate the measurement error variance matrix P ZZ

计算误差协方差矩阵PXZ Calculate the error covariance matrix P XZ

计算增益矩阵Calculate the gain matrix

状态估计值state estimate

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

k=k+1 (83)k=k+1 (83)

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

Claims (1)

1.一种多模型自校准秩滤波方法,其特征在于:它包含以下六个步骤:1. A multi-model self-calibration rank filtering method is characterized in that: it comprises the following six steps: 步骤一:建立系统基本方程Step 1: Establish the basic equations of the system 多模型自校准秩滤波采用自校准秩滤波与秩滤波两种方法进行运算,故系统包含两个状态方程,第一个为含有未知输入项的状态方程,第二个为标准的非线性状态方程,其具体表达式为The multi-model self-calibration rank filter adopts two methods of self-calibration rank filter and rank filter to operate, so the system contains two state equations, the first is a state equation with unknown input items, and the second is a standard nonlinear state equation , its concrete expression is <mrow> <msubsup> <mi>X</mi> <mi>k</mi> <mn>1</mn> </msubsup> <mo>=</mo> <msub> <mi>f</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> <mo>)</mo> </mrow> <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>1</mn></msubsup><mo>=</mo><msub><mi>f</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mrow><mo>(</mo><msubsup><mi>X</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>1</mn></msubsup><mo>)</mo></mrow><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>f</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <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> <mrow><msubsup><mi>X</mi><mi>k</mi><mn>2</mn></msubsup><mo>=</mo><msub><mi>f</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mrow><mo>(</mo><msubsup><mi>X</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>2</mn></msubsup><mo>)</mo></mrow><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=hk(Xk)+Vk·············(3)Z k =h k (X k )+V k ··············(3) 式中,Xk表示系统的状态向量,分别对应含未知输入的动力学模型和标准的动力学模型,Zk表示系统量测向量,fk(·)和hk(·)分别为非线性状态递推方程和量测方程,bk表示未知输入,Wk与Vk分别为系统噪声向量和量测噪声向量,其方差矩阵分别为Qk和Rk,并且满足where X k represents the state vector of the system, and Corresponding to the dynamic model with unknown input and the standard dynamic model, Z k represents the system measurement vector, f k ( ) and h k ( ) are the nonlinear state recurrence equation and measurement equation, b k Represents an 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 <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> <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> 式中,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 and initialize the system 设定状态估计与估计误差方差矩阵的初始值为Set the initial value of the state estimation and estimation error variance matrix to be <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><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>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> <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> 同时,为了完成两模型估计结果的融合,还需要设定两种模型的概率初始值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和PrminAnd the probability initial values Pr max and Pr min for iterative calculation; 针对本发明只选取两个动力学模型且采用最高概率法选取先验估计值,只需要定性分析两种模型的概率大小而不需要精确计算概率值的特点,多模型自校准卡尔曼滤波方法不再使用计算得到的条件概率值进行迭代,而是设定两个确定的概率初始值Prmin和Prmax=1-Prmin;在每一步滤波之前,通过比较上一步概率结果的大小将其分别赋给两个模型,并以它们为初始值更新当前时刻的模型概率;由于Prmin并不是概率下限那样的极小值,因此能保证概率恢复的速度,从而使卡尔曼滤波的实时性得到保证;In view of the fact that the present invention only selects two dynamic models and adopts the highest probability method to select a priori estimated value, it only needs to qualitatively analyze the probability of the two models without accurately calculating the probability value. The multi-model self-calibration Kalman filter method does not Then use the calculated conditional probability value to iterate, but set two determined probability initial values Pr min and Pr max = 1-Pr min ; before each step of filtering, compare the size of the probability result of the previous step to separate them Assign two models, and use them as initial values to update the model probability at the current moment; since Pr min is not a minimum value like the lower limit of the probability, it can guarantee the speed of probability recovery, so that the real-time performance of Kalman filtering is guaranteed ; 步骤三:对系统进行时间更新Step 3: Update the system time 设k-1时刻的状态估计值和误差方差矩阵分别为和Pk-1,基于它们对系统进行时间更新,即计算k时刻的状态一步预测和一步预测误差方差矩阵Pk/k-1;基于秩滤波的一般性递推公式,在时间更新过程中首先需要计算秩采样点集{χi}Suppose the state estimation value and error variance matrix at time k-1 are respectively and P k-1 , based on them to update the system in time, that is, to calculate the one-step prediction of the state at time k and one-step prediction error variance matrix P k/k-1 ; based on the general recursive formula of rank filtering, it is first necessary to calculate the rank sampling point set {χ i } in the time update process 当k=1,2时when k=1,2 状态一步预测值state one-step predictive value <mrow> <msub> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>2</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>2</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>2</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>3</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>3</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>3</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>4</mn> <mi>n</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mn>......</mn> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow> <mrow><msub><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></msub><mo>=</mo><mfenced open = "{" close = ""><mtable><mtr><mtd><mrow><msub><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msub><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></msqrt><mo>)</mo></mrow><mi>i</mi></msub></mo>mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mi>n</mi></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>-</mo><msub><mi>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msub><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>2</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msub><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>2</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>2</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>3</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><mrow><msub><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>-</mo><msub><mi>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msub><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>3</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>3</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>4</mn><mi>n</mi></mrow></mtd></mtr></mtable></mfenced><mn>......</mn><mrow><mo>(</mo><mn>8</mn><mo>)</mo></mrow></mrow> Xk/k-1,i=f(χk-1,i)············(9)X k/k-1,i =f(χ k-1,i )············(9) <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> <mfrac> <mn>1</mn> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>X</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow> <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><mfrac><mn>1</mn><mrow><mn>4</mn><mi>n</mi></mrow></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><msub><mi>X</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></msub><mn>...</mn><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow> 式中,n为状态向量Xk的维度,标准正态偏量表示Pk-1平方根的第i列向量;In the formula, n is the dimension of the state vector X k , and the standard normal deviation represents the ith column vector of the square root of P k-1 ; 一步预测误差方差矩阵One-step forecast error variance matrix <mrow> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&amp;omega;</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </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> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>X</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </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> </mrow> <mi>T</mi> </msup> <mo>}</mo> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mn>....</mn> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow> <mrow><msub><mi>P</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>=</mo><mfrac><mn>1</mn><mi>&amp;omega;</mi></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><mo>{</mo><mrow><mo>(</mo><msub><mi>X</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></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></mrow><msup><mrow><mo>(</mo><msub><mi>X</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></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></mrow><mi>T</mi></msup><mo>}</mo><mo>+</mo><msub><mi>Q</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mn>....</mn><mrow><mo>(</mo><mn>11</mn><mo>)</mo></mrow></mrow> 式中,ω为协方差权重系数,其计算公式为In the formula, ω is the covariance weight coefficient, and its calculation formula is <mrow> <mi>&amp;omega;</mi> <mo>=</mo> <mn>2</mn> <mrow> <mo>(</mo> <msubsup> <mi>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mo>=</mo> <mn>3.0105...</mn> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow> <mrow><mi>&amp;omega;</mi><mo>=</mo><mn>2</mn><mrow><mo>(</mo><msubsup><mi>u</mo>mi><msub><mi>p</mi><mn>1</mn></msub><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>u</mi><msub><mi>p</mi><mn>2</mn></msub><mn>2</mn></msubsup><mo>)</mo></mrow><mo>=</mo><mn>3.0105...</mn><mrow><mo>(</mo><mn>12</mn><mo>)</mo></mrow></mrow> 当k>2时When k>2 状态一步预测值state one-step predictive 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>13</mn> <mo>)</mo> </mrow> </mrow> <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>13</mn><mo>)</mo></mrow></mrow> 一步预测误差方差矩阵One-step forecast error variance 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>14</mn> <mo>)</mo> </mrow> </mrow> <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>14</mn><mo>)</mo></mrow></mrow> 式中In the formula J=argmaxjPr(j|Zk)···········(15)J=argmax j Pr(j|Z k )···········(15) <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> <mo>,</mo> <mi>i</mi> </mrow> <mn>1</mn> </msubsup> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <mo>+</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <mo>-</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>2</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <mo>+</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>2</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>2</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>3</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> <mo>-</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>2</mn> </mrow> <mn>1</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>3</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>3</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>4</mn> <mi>n</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mn>.....</mn> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow> <mrow><msubsup><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>-</mo><mn>2</mn><mo>,</mo><mi>i</mi></mrow><mn>1</mn></msubsup><mo>=</mo><mfenced open = "{" close = ""><mtable><mtr><mtd><mrow><msubsup><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>2</mn></mrow><mn>1</mn></msubsup><mo>+</mo><msub><mi>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>2</mn></mrow><mn>1</mn></msubsup></msqrt><mo>)</mo></mrow><mi>i</mi></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mi>n</mi></mrow></mtd></mtr><mtr><mtd><mrow><msubsup><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>2</mn></mrow><mn>1</mn></msubsup><mo>-</mo><msub><mi>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>2</mn></mrow><mn>1</mn></msubsup></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>2</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><mrow><msubsup><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>2</mn></mrow><mn>1</mn></msubsup><mo>+</mo><msub><mi>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>2</mn></mrow><mn>1</mn></msubsup></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>2</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>2</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>3</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><mrow><msubsup><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>2</mn></mrow><mn>1</mn></msubsup><mo>-</mo><msub><mi>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>2</mn></mrow><mn>1</mn></msubsup></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>3</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>3</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>4</mn><mi>n</mi></mrow></mtd></mtr></mtable></mfenced><mn>.....</mn><mrow><mo>(</mo><mn>16</mn><mo>)</mo></mrow></mrow> <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mn>1</mn> </msubsup> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <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>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <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>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>2</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <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>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>2</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>2</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>3</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <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>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>1</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>3</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>3</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>4</mn> <mi>n</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mn>......</mn> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow> <mrow><msubsup><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mn>1</mn></msubsup><mo>=</mo><mfenced open = "{" close = ""><mtable><mtr><mtd><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>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>1</mn></msubsup></msqrt><mo>)</mo></mrow><mi>i</mi></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mi>n</mi></mrow></mtd></mtr><mtr><mtd><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>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>1</mn></msubsup></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>2</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><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>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>1</mn></msubsup></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>2</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>2</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>3</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><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>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>1</mn></msubsup></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>3</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>3</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>4</mn><mi>n</mi></mrow></mtd></mtr></mtable></mfenced><mn>......</mn><mrow><mo>(</mo><mn>17</mn><mo>)</mo></mrow></mrow> <mrow> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mn>1</mn> </msubsup> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mn>1</mn> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mn>1</mn> </msubsup> <mo>-</mo> <mi>f</mi> <mrow> <mo>(</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mn>1</mn> </msubsup> <mo>)</mo> </mrow> <mn>...</mn> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow> <mrow><msubsup><mi>X</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mn>1</mn></msubsup><mo>=</mo><mi>f</mi><mrow><mo>(</mo><msubsup><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mn>1</mn></msubsup><mo>)</mo></mrow><mo>+</mo><msubsup><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mn>1</mn></msubsup><mo>-</mo><mi>f</mi><mrow><mo>(</mo><msubsup><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mn>1</mn></msubsup><mo>)</mo></mrow><mn>...</mn><mrow><mo>(</mo><mn>18</mn><mo>)</mo></mrow></mrow> <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>+</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>-</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>2</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>+</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>2</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>2</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>3</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> <mo>-</mo> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mn>2</mn> </msubsup> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>3</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>3</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>4</mn> <mi>n</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mn>......</mn> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow> <mrow><msubsup><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mn>2</mn></msubsup><mo>=</mo><mfenced open = "{" close = ""><mtable><mtr><mtd><mrow><msubsup><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>2</mn></msubsup><mo>+</mo><msub><mi>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>2</mn></msubsup></msqrt><mo>)</mo></mrow><mi>i</mi></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mi>n</mi></mrow></mtd></mtr><mtr><mtd><mrow><msubsup><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>2</mn></msubsup><mo>-</mo><msub><mi>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>2</mn></msubsup></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>2</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><mrow><msubsup><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>2</mn></msubsup><mo>+</mo><msub><mi>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>2</mn></msubsup></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>2</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>2</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>3</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><mrow><msubsup><mover><mi>X</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>2</mn></msubsup><mo>-</mo><msub><mi>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msubsup><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mn>2</mn></msubsup></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>3</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>3</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>4</mn><mi>n</mi></mrow></mtd></mtr></mtable></mfenced><mn>......</mn><mrow><mo>(</mo><mn>19</mn><mo>)</mo></mrow></mrow> <mrow> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mn>2</mn> </msubsup> <mo>)</mo> </mrow> <mn>...</mn> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow> <mrow><msubsup><mi>X</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mn>2</mn></msubsup><mo>=</mo><mi>f</mi><mrow><mo>(</mo><msubsup><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mn>2</mn></msubsup><mo>)</mo></mrow><mn>...</mn><mrow><mo>(</mo><mn>20</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> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mi>j</mi> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>21</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><mi>j</mi></msubsup><mo>=</mo><mfrac><mn>1</mn><mrow><mn>4</mn><mi>n</mi></mrow></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><msubsup><mi>X</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mi>j</mi></msubsup><mn>...</mn><mrow><mo>(</mo><mn>21</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> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&amp;omega;</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <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> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <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> </mrow> <mi>T</mi> </msup> <mo>}</mo> <mo>+</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>22</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><mi>j</mi></msubsup><mo>=</mo><mfrac><mn>1</mn><mi>&amp;omega;</mi></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><mo>{</mo><mrow><mo>(</mo><msubsup><mi>X</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><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></mrow><msup><mrow><mo>(</mo><msubsup><mi>X</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><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></mrow><mi>T</mi></msup><mo>}</mo><mo>+</mo><msub><mi>Q</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mn>...</mn><mrow><mo>(</mo><mn>22</mn><mo>)</mo></mn>mrow></mrow> 其中,函数argmax[f(x)]返回当f(x)最大时x的值,并且where the function argmax[f(x)] returns the value of 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>23</mn> <mo>)</mo> </mrow> </mrow> <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>23</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>24</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>24</mn><mo>)</mo></mrow></mrow> 在上述计算过程中,式(18)和式(21)提供自校准状态方程的一步预测结果,式(20)和式(21)提供标准状态方程计算的一步预测结果,然后通过式(15)中概率大小的比对,由式(13)完成最终的一步预测值筛选;概率大小的计算依托于贝叶斯原理,比较的是在当前时刻量测值已知的情况下,两种模型的条件概率,如式(23)和式(24)所示;In the above calculation process, formula (18) and formula (21) provide the one-step prediction result of the self-calibration state equation, and formula (20) and formula (21) provide the one-step prediction result of the standard state equation calculation, and then through the formula (15) The comparison of the medium probability is done by the formula (13) to complete the final one-step prediction value screening; the calculation of the probability is based on the Bayesian principle, and the comparison is when the measured value at the current moment is known. Conditional probability, as shown in formula (23) and formula (24); 步骤四:迭代变量更新Step 4: Iterative variable update 在步骤三中有很多中间变量需要实时更新,因此有必要得到它们的递推公式,进而保证整个滤波过程的顺利进行;这些迭代变量包括:In step three, 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 <mrow> <msubsup> <mi>K</mi> <mi>k</mi> <mi>j</mi> </msubsup> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>X</mi> <mi>Z</mi> </mrow> <mi>j</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mrow> <mi>Z</mi> <mi>Z</mi> </mrow> <mi>j</mi> </msubsup> <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><mi>K</mi><mi>k</mi><mi>j</mi></msubsup><mo>=</mo><msubsup><mi>P</mi><mrow><mi>X</mi><mi>Z</mi></mrow><mi>j</mi></msubsup><msup><mrow><mo>(</mo><msubsup><mi>P</mi><mrow><mi>Z</mi><mi>Z</mi></mrow><mi>j</mi></msubsup><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> <msubsup> <mover> <mi>Z</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><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><msubsup><mover><mi>Z</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> <msubsup> <mi>P</mi> <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> <msubsup> <mi>P</mi> <mrow> <mi>Z</mi> <mi>Z</mi> </mrow> <mi>j</mi> </msubsup> <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> <mrow><msubsup><mi>P</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><mo>-</mo><msubsup><mi>K</mi><mi>k</mi><mi>j</mi></msubsup><msubsup><mi>P</mi><mrow><mi>Z</mi><mi>Z</mi></mrow><mi>j</mi></msubsup><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> 式中In the formula <mrow> <msubsup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mi>j</mi> </msubsup> <mo>)</mo> </mrow> <mn>...</mn> <mrow> <mo>(</mo> <mn>28</mn> <mo>)</mo> </mrow> </mrow> <mrow><msubsup><mi>Z</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mi>j</mi></msubsup><mo>=</mo><mi>h</mi><mrow><mo>(</mo><msubsup><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mi>j</mi></msubsup><mo>)</mo></mrow><mn>...</mn><mrow><mo>(</mo><mn>28</mn><mo>)</mo></mrow></mrow> <mrow> <msubsup> <mover> <mi>Z</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> <mfrac> <mn>1</mn> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <msubsup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mi>j</mi> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>29</mn> <mo>)</mo> </mrow> </mrow> <mrow><msubsup><mover><mi>Z</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><mfrac><mn>1</mn><mrow><mn>4</mn><mi>n</mi></mrow></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><msubsup><mi>Z</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mi>j</mi></msubsup><mn>...</mn><mrow><mo>(</mo><mn>29</mn><mo>)</mo></mrow></mrow> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>Z</mi> <mi>Z</mi> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&amp;omega;</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <mrow> <mo>(</mo> <msubsup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mi>j</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>Z</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> <msup> <mrow> <mo>(</mo> <msubsup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mi>j</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>Z</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> <mi>T</mi> </msup> <mo>}</mo> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mn>....</mn> <mrow> <mo>(</mo> <mn>30</mn> <mo>)</mo> </mrow> </mrow> <mrow><msubsup><mi>P</mi><mrow><mi>Z</mi><mi>Z</mi></mrow><mi>j</mi></msubsup><mo>=</mo><mfrac><mn>1</mn><mi>&amp;omega;</mi></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><mo>{</mo><mrow><mo>(</mo><msubsup><mi>Z</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mi>j</mi></msubsup><mo>-</mo><msubsup><mover><mi>Z</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><msup><mrow><mo>(</mo><msubsup><mi>Z</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><mi>j</mi></msubsup><mo>-</mo><msubsup><mover><mi>Z</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><mi>T</mi></msup><mo>}</mo><mo>+</mo><msub><mi>R</mi><mi>k</mi></msub><mn>....</mn><mrow><mo>(</mo><mn>30</mn><mo>)</mo></mrow></mrow> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>X</mi> <mi>Z</mi> </mrow> <mi>j</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&amp;omega;</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <mrow> <mo>(</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <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> </mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>Z</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> <mi>j</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>}</mo> <mn>.....</mn> <mrow> <mo>(</mo> <mn>31</mn> <mo>)</mo> </mrow> </mrow> <mrow><msubsup><mi>P</mi><mrow><mi>X</mi><mi>Z</mi></mrow><mi>j</mi></msubsup><mo>=</mo><mfrac><mn>1</mn><mi>&amp;omega;</mi></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><mo>{</mo><mrow><mo>(</mo><msubsup><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow><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></mrow><msup><mrow><mo>(</mo><msubsup><mi>Z</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi>mi></mrow><mi>j</mi></msubsup><mo>-</mo><msubsup><mover><mi>Z</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mi>T</mi></msubsup><mo>)</mo></mrow><mi>T</mi></msup><mo>}</mo><mn>.....</mn><mrow><mo>(</mo><mn>31</mn><mo>)</mo></mrow></mrow> 各模型条件概率重置Conditional probability reset for each model Pr(J|Zk)=Prmax············(32)Pr(J|Z k )=Pr max ·············(32) Pr[(3-J)|Zk]=Prmin···········(33);Pr[(3-J)|Z k ]=Pr min ············(33); 步骤五:量测更新Step 5: Measurement update 首先,基于状态一步预测和一步预测误差方差矩阵Pk/k-1,根据秩滤波的一般性递推公式对系统状态进行重采样,得到新的秩采样点集{χk/k-1,i}First, one-step prediction based on state and one-step prediction error variance matrix P k/k-1 , resample the system state according to the general recursive formula of rank filtering, and obtain a new rank sampling point set {χ k/k-1,i } <mrow> <msub> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <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> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <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> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>1</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>2</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <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> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>2</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>2</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>3</mn> <mi>n</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <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> <msub> <mi>u</mi> <msub> <mi>p</mi> <mn>2</mn> </msub> </msub> <msub> <mrow> <mo>(</mo> <msqrt> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </msqrt> <mo>)</mo> </mrow> <mrow> <mi>i</mi> <mo>-</mo> <mn>3</mn> <mi>n</mi> </mrow> </msub> </mrow> </mtd> <mtd> <mrow> <mi>i</mi> <mo>=</mo> <mn>3</mn> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mn>4</mn> <mi>n</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> <mn>.....</mn> <mrow> <mo>(</mo> <mn>34</mn> <mo>)</mo> </mrow> </mrow> <mrow><msub><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></msub><mo>=</mo><mfenced open = "{" close = ""><mtable><mtr><mtd><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><msub><mi>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msub><mi>P</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></msqrt><mo>)</mo></mrow><mi>i</mi></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>1</mn><mo>,</mo><mn>...</mtd>mn><mo>,</mo><mi>n</mi></mrow></mtd></mtr><mtr><mtd><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><msub><mi>u</mi><msub><mi>p</mi><mn>1</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msub><mi>P</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>2</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><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><msub><mi>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msub><mi>P</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>2</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>2</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>3</mn><mi>n</mi></mrow></mtd></mtr><mtr><mtd><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><msub><mi>u</mi><msub><mi>p</mi><mn>2</mn></msub></msub><msub><mrow><mo>(</mo><msqrt><msub><mi>P</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></msqrt><mo>)</mo></mrow><mrow><mi>i</mi><mo>-</mo><mn>3</mn><mi>n</mi></mrow></msub></mrow></mtd><mtd><mrow><mi>i</mi><mo>=</mo><mn>3</mn><mi>n</mi><mo>+</mo><mn>1</mn><mo>,</mo><mn>...</mn><mo>,</mo><mn>4</mn><mi>n</mi></mrow></mtd></mtr></mtable></mfenced><mn>.....</mn><mrow><mo>(</mo><mn>34</mn><mo>)</mo></mrow></mrow> 然后,根据{χk/k-1,i}计算量测估计值 Then, calculate the measurement estimate according to {χ k/k-1,i } Zk/k-1,i=h(χk/k-1,i)···········(35)Z k/k-1,i =h(χ k/k-1,i )···········(35) <mrow> <msub> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mn>...</mn> <mrow> <mo>(</mo> <mn>36</mn> <mo>)</mo> </mrow> </mrow> <mrow><msub><mover><mi>Z</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>=</mo><mfrac><mn>1</mn><mrow><mn>4</mn><mi>n</mi></mrow></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><msub><mi>Z</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></msub><mn>...</mn><mrow><mo>(</mo><mn>36</mn><mo>)</mo></mrow></mrow> 由式(35)和式(36)计算量测误差方差矩阵PZZ Calculate the measurement error variance matrix P ZZ by formula (35) and formula (36) <mrow> <msub> <mi>P</mi> <mrow> <mi>Z</mi> <mi>Z</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&amp;omega;</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>}</mo> <mo>+</mo> <msub> <mi>R</mi> <mi>k</mi> </msub> <mn>.....</mn> <mrow> <mo>(</mo> <mn>37</mn> <mo>)</mo> </mrow> </mrow> <mrow><msub><mi>P</mi><mrow><mi>Z</mi><mi>Z</mi></mrow></msub><mo>=</mo><mfrac><mn>1</mn><mi>&amp;omega;</mi></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><mo>{</mo><mrow><mo>(</mo><msub><mi>Z</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></msub><mo>-</mo><msub><mover><mi>Z</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><msup><mrow><mo>(</mo><msub><mi>Z</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></msub><mo>-</mo><msub><mover><mi>Z</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mi>T</mi></msup><mo>}</mo><mo>+</mo><msub><mi>R</mi><mi>k</mi></msub><mn>.....</mn><mrow><mo>(</mo><mn>37</mn><mo>)</mo></mrow></mrow> 由式(34)、式(35)和式(36)计算误差协方差矩阵PXZ Calculate the error covariance matrix P XZ by formula (34), formula (35) and formula (36) <mrow> <msub> <mi>P</mi> <mrow> <mi>X</mi> <mi>Z</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>&amp;omega;</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mrow> <mn>4</mn> <mi>n</mi> </mrow> </munderover> <mo>{</mo> <mrow> <mo>(</mo> <msub> <mi>&amp;chi;</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </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> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>-</mo> <msub> <mover> <mi>Z</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>/</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>}</mo> <mn>.....</mn> <mrow> <mo>(</mo> <mn>38</mn> <mo>)</mo> </mrow> </mrow> <mrow><msub><mi>P</mi><mrow><mi>X</mi><mi>Z</mi></mrow></msub><mo>=</mo><mfrac><mn>1</mn><mi>&amp;omega;</mi></mfrac><munderover><mo>&amp;Sigma;</mo><mrow><mi>i</mi><mo>=</mo><mn>1</mn></mrow><mrow><mn>4</mn><mi>n</mi></mrow></munderover><mo>{</mo><mrow><mo>(</mo><msub><mi>&amp;chi;</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></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></mrow><msup><mrow><mo>(</mo><msub><mi>Z</mi><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn><mo>,</mo><mi>i</mi></mrow></msub><mo>-</mo><msub><mover><mi>Z</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>/</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>)</mo></mrow><mi>T</mi></msup><mo>}</mo><mn>.....</mn><mrow><mo>(</mo><mn>38</mn><mo>)</mo></mrow></mrow> 由式(37)和式(38)计算增益矩阵Calculate the gain matrix by formula (37) and formula (38) <mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>P</mi> <mrow> <mi>X</mi> <mi>Z</mi> </mrow> </msub> <msubsup> <mi>P</mi> <mrow> <mi>Z</mi> <mi>Z</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mn>...</mn> <mrow> <mo>(</mo> <mn>39</mn> <mo>)</mo> </mrow> </mrow> <mrow><msub><mi>K</mi><mi>k</mi></msub><mo>=</mo><msub><mi>P</mi><mrow><mi>X</mi><mi>Z</mi></mrow></msub><msubsup><mi>P</mi><mrow><mi>Z</mi><mi>Z</mi></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msubsup><mn>...</mn><mrow><mo>(</mo><mn>39</mn><mo>)</mo></mrow></mrow> 则,由量测更新得到最终的状态估计值和状态估计误差方差矩阵Then, the final state estimation value and state estimation error variance matrix are obtained by the measurement update <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> <mover> <mi>Z</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>40</mn> <mo>)</mo> </mrow> <mo>;</mo> </mrow> <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><mover><mi>Z</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>40</mn><mo>)</mo></mrow><mo>;</mo></mrow> 步骤六:迭代计算Step 6: Iterative calculation 根据k时刻的状态估计值和误差方差矩阵Pk,重复步骤三、步骤四和步骤五,进而得到k+1时刻的状态估计值和误差方差矩阵,往复迭代,直至滤波过程结束。According to the estimated value of the state at time k and error variance matrix P k , repeat steps 3, 4, and 5 to obtain the state estimate and error variance matrix at time k+1, and iterate back and forth until the filtering process ends.
CN201710853216.6A 2017-09-20 2017-09-20 A kind of multi-model self calibration order filtering method Pending CN107807906A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710853216.6A CN107807906A (en) 2017-09-20 2017-09-20 A kind of multi-model self calibration order filtering method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710853216.6A CN107807906A (en) 2017-09-20 2017-09-20 A kind of multi-model self calibration order filtering method

Publications (1)

Publication Number Publication Date
CN107807906A true CN107807906A (en) 2018-03-16

Family

ID=61584288

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710853216.6A Pending CN107807906A (en) 2017-09-20 2017-09-20 A kind of multi-model self calibration order filtering method

Country Status (1)

Country Link
CN (1) CN107807906A (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110110711A (en) * 2019-06-06 2019-08-09 郑州轻工业学院 A kind of iterative learning control systems input signal estimation method under noisy communication channel
CN111158382A (en) * 2020-01-19 2020-05-15 郑州轻工业大学 Construction method and system of unmanned vehicle positioning model based on wireless ultra-broadband network
CN111189454A (en) * 2020-01-09 2020-05-22 郑州轻工业大学 Unmanned vehicle SLAM navigation method based on rank Kalman filtering
CN113091748A (en) * 2021-04-12 2021-07-09 北京航空航天大学 Indoor self-calibration navigation positioning method

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
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
CN111189454A (en) * 2020-01-09 2020-05-22 郑州轻工业大学 Unmanned vehicle SLAM navigation method based on rank Kalman filtering
CN111158382A (en) * 2020-01-19 2020-05-15 郑州轻工业大学 Construction method and system of unmanned vehicle positioning model based on wireless ultra-broadband network
CN113091748A (en) * 2021-04-12 2021-07-09 北京航空航天大学 Indoor self-calibration navigation positioning method

Similar Documents

Publication Publication Date Title
CN111985093B (en) An Adaptive Unscented Kalman Filter State Estimation Method with Noise Estimator
CN109990786A (en) Mobile target tracking method and device
CN107783944A (en) A kind of multi-model self calibration Unscented kalman filtering method
CN107547067A (en) A kind of multi-model self calibration EKF method
CN107807906A (en) A kind of multi-model self calibration order filtering method
CN103955892B (en) Unscented kalman filtering method, device are blocked in a kind of method for tracking target and expansion
CN104112079A (en) Fuzzy adaptive variational Bayesian unscented Kalman filter method
CN106487358A (en) A kind of maximal correlation entropy volume kalman filter method based on statistical linear regression
CN109325128B (en) Method and system for tracking maneuvering target
CN105205313A (en) Fuzzy Gaussian sum particle filtering method and device as well as target tracking method and device
CN110119588B (en) Online Optimal Design Method Based on Extended Kalman Filter State Estimate
CN106767780A (en) Based on the extension ellipsoid set-membership filtering method that Chebyshev polynomial interopolations are approached
Xu et al. Convergence analysis of a synchronous gradient estimation scheme for time-varying parameter systems
CN107565931A (en) A kind of self calibration Unscented kalman filtering method
CN113032988A (en) High-order extended Kalman filter design method based on maximum correlation entropy
Machado et al. Takagi–Sugeno fuzzy models in the framework of orthonormal basis functions
CN103400158B (en) Based on the level set tracking of dynamic shape code book study
CN108871365B (en) State estimation method and system under course constraint
Otto et al. Learning Bilinear Models of Actuated Koopman Generators from Partially Observed Trajectories
CN107632959A (en) A kind of multi-model self calibration kalman filter method
CN109000638A (en) A kind of small field of view star sensor measurement filtering wave by prolonging time method
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
CN113432608A (en) Generalized high-order CKF algorithm based on maximum correlation entropy and suitable for INS/CNS integrated navigation system
Wei et al. A Pearson-Type VII Distribution With Adaptive Parameters Selection-Based Interacting Multiple Model Kalman Filter

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
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20180316

WD01 Invention patent application deemed withdrawn after publication