CN104880189A - Low-cost tracking anti-jamming method of antenna for satellite communication in motion - Google Patents

Abstract

The invention relates to a low-cost tracking anti-jamming method of an antenna for satellite communication in motion, aiming at solving the technical problems that the existing antenna for satellite communication in motion can be easily interfered and cannot be measured accurately. A kalman filter is adopted to fuse various kinds of information, accelerometer information is adopted to estimate the carrier attitude, course information is acquired through tracking and the correction of gyroscope information P, and the measurement noise covariance of the filter is adjusted according to the motion condition of the carrier. The method is suitable for tracking antennas in the mobile carriers such as vehicle-mounted antenna, airborne antenna, shipborne antennas and the like.

Description

A kind of antenna for satellite communication in motion low cost follows the tracks of anti-interference method

Technical field

The present invention relates to a kind of antenna tracking method, particularly relate to a kind of antenna for satellite communication in motion low cost and follow the tracks of anti-interference method.

Background technology

Existing being used for obtains in several low cost sensors of inclination angle (Roll and Pitch) and course (Yaw) information.For measurement of dip angle, comprise following a few class: first is inclinator, the output of inclinator is directly proportional to inclination angle (according to gravity field), but this sensor responds comparatively slowly and is subject to the impact of vibrations, therefore can not be used alone in pitch angle is estimated.Second is rate gyro sensor, gyro to measure turning rate.In order to obtain angle information, need to carry out integration to sensor signal.Therefore, even if little rate error also can cause the drift estimating angle.3rd is accelerometer, and the problem of accelerometer is the impact being subject to Maneuver Acceleration, only when just carrying out inclination angle estimation without when Maneuver Acceleration.

For heading measure, we also can pass through integrating gyroscope signal, but can there is drifting problem equally.Another sensor is magnetometer, and it obtains course by measuring terrestrial magnetic field.Although very accurately with small and exquisite, owing to being subject to the impact of magnetic interference, still comparatively big error can be there is in sensor itself.

For MEMS inertial attitude system functionally with the many deficiencies in precision, usually adopt fusion method to be learnt from other's strong points to offset one's weaknesses by multiple sensor.First by the attitude angle that acceleration information and the magnetometer output magnetic field intensity information of accelerometer output calculate, compensate the drift of gyro as measuring value by this attitude angle, namely this algorithm can ensure the precision of determining appearance, can ensure that system has higher dynamic and stability again by gyro.In this mode of operation, adopt gyro, accelerometer and magnetometer to be combined through the attitude of Kalman filtering determination carrier, rely on gravity vector to be the angle drift after reference information have modified gyro integration with ground magnetic vector, improve the precision of aviation attitude system.But utilizing magnetometer data to determine there is magnetic interference in course angle.When sensor is in the depletion region without magnetic interference, terrestrial magnetic field is not interfered, and course angle is now correct.In fact, magnetometer is mounted on car body, and wherein on car body platform, car, element such as engine, cable etc. can generate an electromagnetic field interference.In addition, exterior object is as the car of process, and buildings, high-tension bus-bar, large metal object etc. also can produce electromagnetic interference (EMI).These interference can be upset, magnetic field deviously, finally cause course angle error.

Summary of the invention

The present invention is easily interfered to solve existing antenna for satellite communication in motion, measures inaccurate technical matters.

The invention provides a kind of antenna for satellite communication in motion low cost and follow the tracks of anti-interference method, draw together following steps:

(1) the angular velocity signal ω that MEMS tri-axle gyro sensitive carrier moves is utilized _b, utilize the acceleration f in mems accelerometer sensitive carrier coordinate system _b, the course angle error angle utilizing trace information to obtain, and signal is delivered to controller;

(2) state equation is set up by described controller:

$\stackrel{\·}{X}=f\left(X\right)+w\left(t\right)$

In this state equation, X is state variable, and system noise w is the zero mean Gaussian white noise sequence of random independent, i.e. w ~ N (0, Q);

State variable X is:

$X=\left[\begin{array}{c}q\\ {\mathrm{\ω}}_{\mathrm{bias}}\end{array}\right]={\left[\begin{array}{ccccccc}{q}_0& q_1& q_2& q_3& {\mathrm{\ω}}_{r\_\mathrm{bias}}& {\mathrm{\ω}}_{p\_\mathrm{bias}}& {\mathrm{\ω}}_{y\_\mathrm{bias}}\end{array}\right]}^T$

Wherein, q=[q ₀q ₁q ₂q ₃] ^trepresent attitude four element, ω _bias=[ω _{r_bias}ω _{p_bias}ω _{y_bias}] ^tbe expressed as gyro zero evaluated error partially;

(3) define system state-transition matrix is:

$\mathrm{\Φ}=\frac{\∂f\left(X\right)}{\∂X}$

Wherein, $f\left(X\right)=\left[\begin{array}{c}-0.5(x_2{\mathrm{\ω}}_{\mathrm{bx}}+x_3{\mathrm{\ω}}_{\mathrm{by}}+x_4{\mathrm{\ω}}_{\mathrm{bz}})\\ 0.5(x_1{\mathrm{\ω}}_{\mathrm{bx}}-x_4{\mathrm{\ω}}_{\mathrm{by}}+x_3{\mathrm{\ω}}_{\mathrm{bz}})\\ 0.5(x_4{\mathrm{\ω}}_{\mathrm{bx}}+x_1{\mathrm{\ω}}_{\mathrm{by}}-x_3{\mathrm{\ω}}_{\mathrm{bz}})\\ -0.5(x_3{\mathrm{\ω}}_{\mathrm{bx}}-x_2{\mathrm{\ω}}_{\mathrm{by}}-x_1{\mathrm{\ω}}_{\mathrm{bz}})\\ 0\\ 0\\ 0\end{array}\right]$

Observation equation is set up by controller:

Z=h (X)+v, in this observation equation, Z=[φ θ ψ] ^tfor measuring variable, measurement noise v is the zero mean Gaussian white noise sequence of random independent, i.e. v ~ N (0, R);

Progressive one calculates system measurements transition matrix is:

$H=\frac{\∂h\left(X\right)}{\∂X};$

Two parts are divided into: a part utilizes the weight component of MEMS triaxial accelerometer sensitivity to solve attitude angle by corresponding for observed quantity, course angle information is estimated, respectively the measurement transition matrix H of corresponding two different computation periods after the course angle error obtained of another part trace information and gyro carry out PI correction ₁and H ₂;

Measure transition matrix H ₁process as follows:

The specific force f that what MEMS triaxial accelerometer recorded is suffered by carrier, if in inertial measurement cluster after de-noising to add table measured value be f _x, f _y, f _zwhen carrier is in static and quasistatic, because bearer rate is lower, the impact of Corioli's acceleration and other disturbing accelerations can be ignored, be " east northeast ground " at navigation coordinate, carrier coordinate system is in " under front right " situation, can be obtained the attitude angle of carrier by the measured value adding table according to the following formula;

φ＝-atan(f _y/f _z)，

$\mathrm{\θ}=\arcsin (f_x/\sqrt{f_x^2+f_y^2+f_z^2}),$

Corresponding measurement transition matrix is: $H_1={\left[\begin{array}{ccc}\frac{\∂\mathrm{\φ}}{\∂X}& \frac{\∂\mathrm{\θ}}{\∂X}& 0_{7\×1}\end{array}\right]}^T;$

Measure transition matrix H ₂process as follows:

Adopt trace information to come the course of regular calibration system, trace information can obtain the sensing deviation angle of antenna beam after treatment, by obtaining course angle error after conversion; Course angle error goes to correct gyro value ω after a PI controller _bz, can course angle ψ be obtained by after the gyro value integration after correction; Wherein, the gain of controller, K _pand K _iwith cutoff frequency ω and damping relevant:

K _i＝ω ²

Using course angle ψ also as the measurement variable of Kalman, can obtain measuring transition matrix:

$H_2={\left[\begin{array}{ccc}\frac{\∂\mathrm{\φ}}{\∂X}& \frac{\∂\mathrm{\θ}}{\∂X}& \frac{\∂\mathrm{\ψ}}{\∂X}\end{array}\right]}^T;$

(4) carrier movement condition discrimination;

Will speed up angle value and be converted to navigational coordinate system, according to rule judgment below:

If (| a _nx| > δ _x) | (| a _ny| > δ _y) | (| a _nz-g| > δ _z), assert to there is external acceleration, in formula, a _nx, a _nyand a _nzfor the accekeration under navigational coordinate system;

When carrier exists external acceleration, measure covariance matrix R change filter gain by adjustment, now, surveying covariance matrix is:

$R=\left[\begin{array}{ccc}1000\×r_{\mathrm{\φ}}& 0& 0\\ 0& 1000\×r_{\mathrm{\θ}}& 0\\ 0& 0& r_{\mathrm{\ψ}}\end{array}\right].$

Preferably, the design of wave filter is:

(1) computer card Germania yield value:

$K_k=P_k^{-}{H}_{\mathrm{ki}}^T{(H_{\mathrm{ki}}{P}_k^{-}{H}_{\mathrm{ki}}^T+R_k)}^{-1};$

(2) estimated value is upgraded:

${\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_{\mathrm{ki}}-h_i\left({\hat{X}}_k^{-}\right)),$

(3) matrix of error is upgraded:

$P_k=(I-K_k{H}_{\mathrm{ki}})P_k^{-},$

(4) prediction reference state error:

${\hat{X}}_{k+1}^{-}=f\left({\hat{X}}_k\right),$

(5) estimation error matrix:

$P_{k+1}^{-}={\mathrm{\Φ}}_k{P}_k{\mathrm{\Φ}}_k^T+Q,$

Wherein, when trace information is invalid, measurement matrix is H ₁, measured value corresponds to Z ₁, measuring estimated value is when trace information is effective, measurement matrix is H ₂, measured value corresponds to Z ₂, measuring estimated value is

The invention has the beneficial effects as follows: system have employed Kalman filter to merge various information, accelerometer information is adopted to estimate attitude of carrier, utilize tracking and gyro information PI to correct and obtain course information, and adjust the measuring noise square difference battle array of wave filter according to the motion conditions of carrier.The invention solves low cost tracking and be easily subject to electromagnetic interference (EMI), make the tracking performance of antenna more stable.

Further aspect of the present invention, by the description of following embodiment, is clearly recorded.

Accompanying drawing explanation

Fig. 1 is tracking countermeasure set structured flowchart of the present invention;

Fig. 2 is the attitude measurement method composition frame chart of combination;

Fig. 3 is that PI corrects schematic diagram;

Fig. 4 is filter loop renewal figure.

Reference numeral illustrates:

1.MEMS tri-axle gyro, 2.MEMS three axis accelerometer, 3. satellite beacon receiver, 4. the first filter amplifier, 5. the second filter amplifier, 6.A/D converter, 7. controller.

Embodiment

System is made up of satellite beacon receiver 3, sensor, Acquisition Circuit.Sensor comprises MEMS tri-axle gyro 1 and MEMS triaxial accelerometer 2, under dynamic environment to the measurement of carrier roll angle and the angle of pitch, MEMS tri-axle gyro 1 is for responsive acceleration of gravity, and MEMS triaxial accelerometer 2 is for sensitive carrier angular velocity of satellite motion.Controller 7 gathers the data of MEMS tri-axle gyro 1 and MEMS triaxial accelerometer 2 and utilizes Kalman filter (the first filter amplifier 4 in referring to and the second filter amplifier 5 have come) to merge speed, the acceleration information of gyro.Signal acquisition circuit is made up of parts such as A/D converter 6 and power circuits.

Utilize the angular velocity signal ω that MEMS tri-axle gyro 1 sensitive carrier moves _b, the acceleration f in mems accelerometer 2 sensitive carrier coordinate system _b, the course angle error angle utilizing trace information to obtain, and signal is delivered to controller 7.

As shown in Figure 2, utilize EKF to merge different information, estimated state variable.

State equation is set up by controller 7:

$\stackrel{\·}{X}=f\left(X\right)+w\left(t\right)---\left(1\right)$

In formula (1), X is state variable, and system noise w is the zero mean Gaussian white noise sequence of random independent, i.e. w ~ N (0, Q).

State variable X is:

$X=\left[\begin{array}{c}q\\ {\mathrm{\ω}}_{\mathrm{bias}}\end{array}\right]={\left[\begin{array}{ccccccc}{q}_0& q_1& q_2& q_3& {\mathrm{\ω}}_{r\_\mathrm{bias}}& {\mathrm{\ω}}_{p\_\mathrm{bias}}& {\mathrm{\ω}}_{y\_\mathrm{bias}}\end{array}\right]}^T$

Wherein, q=[q ₀q ₁q ₂q ₃] ^trepresent attitude four element, ω _bias=[ω _{r_bias}ω _{p_bias}ω _{y_bias}] ^tbe expressed as gyro zero evaluated error partially.

Define system state-transition matrix is:

$\mathrm{\Φ}=\frac{\∂f\left(X\right)}{\∂X}---\left(2\right)$

In formula (2), $f\left(X\right)=\left[\begin{array}{c}-0.5(x_2{\mathrm{\ω}}_{\mathrm{bx}}+x_3{\mathrm{\ω}}_{\mathrm{by}}+x_4{\mathrm{\ω}}_{\mathrm{bz}})\\ 0.5(x_1{\mathrm{\ω}}_{\mathrm{bx}}-x_4{\mathrm{\ω}}_{\mathrm{by}}+x_3{\mathrm{\ω}}_{\mathrm{bz}})\\ 0.5(x_4{\mathrm{\ω}}_{\mathrm{bx}}+x_1{\mathrm{\ω}}_{\mathrm{by}}-x_3{\mathrm{\ω}}_{\mathrm{bz}})\\ -0.5(x_3{\mathrm{\ω}}_{\mathrm{bx}}-x_2{\mathrm{\ω}}_{\mathrm{by}}-x_1{\mathrm{\ω}}_{\mathrm{bz}})\\ 0\\ 0\\ 0\end{array}\right]$

Observation equation is set up by controller 7:

Z＝h(X)+v (3)

In formula (3), Z=[φ θ ψ] ^tfor measuring variable, measurement noise v is the zero mean Gaussian white noise sequence of random independent, i.e. v ~ N (0, R).

System measurements transition matrix is:

$H=\frac{\∂h\left(X\right)}{\∂X}---\left(4\right)$

The renewal frequency considering trace information is comparatively slow, slow a lot of than MEMS triaxial accelerometer 2.Thus, algorithm is divided into two parts by corresponding for observed quantity: a part utilizes the weight component of MEMS triaxial accelerometer 2 sensitivity to solve attitude angle (roll angle and pitch angle), course angle information is estimated, respectively the measurement transition matrix H of corresponding two different computation periods after the course angle error obtained of another part trace information and gyro carry out PI correction ₁and H ₂.

Measure transition matrix H ₁process as follows:

The specific force f that what MEMS triaxial accelerometer 2 recorded is suffered by carrier, if in inertial measurement cluster after de-noising to add table measured value be f _x, f _y, f _zwhen carrier is in static and quasistatic, because bearer rate is lower, the impact of Corioli's acceleration and other disturbing accelerations can be ignored, be " east northeast ground " at navigation coordinate, carrier coordinate system is in " under front right " situation, can be obtained the attitude angle of carrier by the measured value adding table according to the following formula.

φ＝-atan(f _y/f _z) (5)

$\mathrm{\θ}=\arcsin (f_x/\sqrt{f_x^2+f_y^2+f_z^2})$

Corresponding measurement transition matrix is: $H_1={\left[\begin{array}{ccc}\frac{\∂\mathrm{\φ}}{\∂X}& \frac{\∂\mathrm{\θ}}{\∂X}& 0_{7\×1}\end{array}\right]}^T$

Measure transition matrix H ₂process as follows:

Because MEMS triaxial accelerometer 2 is unable to estimate course angle, the course of carrier can be also disperse for a long time.In order to avoid course is dispersed, trace information is adopted to come the course of regular calibration system.Trace information can obtain the sensing deviation angle of antenna beam after treatment, by obtaining course angle error after conversion.Through a PI controller, (gain is respectively K to course angle error _pand K _i) after go correct gyro value ω _bz, can course angle ψ be obtained by after the gyro value integration after correction.Wherein, the gain of controller, K _pand K _iwith cutoff frequency ω and damping relevant:

K _i＝ω ²

Using course angle ψ also as the measurement variable of Kalman, can obtain measuring transition matrix:

$H_2={\left[\begin{array}{ccc}\frac{\∂\mathrm{\φ}}{\∂X}& \frac{\∂\mathrm{\θ}}{\∂X}& \frac{\∂\mathrm{\ψ}}{\∂X}\end{array}\right]}^T$

In carrier movement, according to the attitude that MEMS tri-axle gyro 1 and MEMS triaxial accelerometer 3 are estimated, there is larger dynamic error.In order to reduce the impact that dynamic dynamic environment is estimated attitude of carrier, system needs the change automatically adapting to maneuvering condition.Will speed up angle value and be converted to navigational coordinate system, according to rule judgment below:

If (| a _nx| > δ _x) | (| a _ny| > δ _y) | (| a _nz-g| > δ _z), assert to there is external acceleration.

In formula, a _nx, a _nyand a _nzfor the accekeration under navigational coordinate system.

When carrier exists external acceleration, measure covariance matrix R by adjustment and change filter gain.Now, surveying covariance matrix is:

$R=\left[\begin{array}{ccc}1000\×r_{\mathrm{\φ}}& 0& 0\\ 0& 1000\×r_{\mathrm{\θ}}& 0\\ 0& 0& r_{\mathrm{\ψ}}\end{array}\right]$

Introduce the design of wave filter below:

(1) computer card Germania yield value:

$K_k=P_k^{-}{H}_{\mathrm{ki}}^T{(H_{\mathrm{ki}}{P}_k^{-}{H}_{\mathrm{ki}}^T+R_k)}^{-1}$

(2) estimated value is upgraded:

${\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_{\mathrm{ki}}-h_i\left({\hat{X}}_k^{-}\right))$

(3) matrix of error is upgraded

$P_k=(I-K_k{H}_{\mathrm{ki}})P_k^{-}$

(4) prediction reference state error

${\hat{X}}_{k+1}^{-}=f\left({\hat{X}}_k\right)$

(5) estimation error matrix

$P_{k+1}^{-}={\mathrm{\Φ}}_k{P}_k{\mathrm{\Φ}}_k^T+Q$

Wherein, when trace information is invalid, measurement matrix is H ₁, measured value corresponds to Z ₁, measuring estimated value is when trace information is effective, measurement matrix is H ₂, measured value corresponds to Z ₂, measuring estimated value is

The above, only to the preferred embodiments of the present invention, is not limited to the present invention, and for a person skilled in the art, the present invention can have various modifications and variations.Every in claim limited range of the present invention, any amendment made, equivalent replacement, improvement etc., all should within protection scope of the present invention.

Claims (2)

1. antenna for satellite communication in motion low cost follows the tracks of an anti-interference method, it is characterized in that comprising the following steps:

(1) the angular velocity signal ω that MEMS tri-axle gyro sensitive carrier moves is utilized _b, utilize the acceleration f in mems accelerometer sensitive carrier coordinate system _b, the course angle error angle utilizing trace information to obtain, and signal is delivered to controller;

(2) state equation is set up by described controller:

$\stackrel{\·}{X}=f\left(X\right)+w\left(t\right)$

In this state equation, X is state variable, and system noise w is the zero mean Gaussian white noise sequence of random independent, i.e. w ~ N (0, Q);

State variable X is:

$X=\left[\begin{array}{c}q\\ {\mathrm{\ω}}_{\mathrm{bias}}\end{array}\right]{\left[\begin{array}{ccccccc}{q}_0& q_1& q_2& q_3& {\mathrm{\ω}}_{r\_\mathrm{bias}}& {\mathrm{\ω}}_{p\_\mathrm{bias}}& {\mathrm{\ω}}_{y\_\mathrm{bias}}\end{array}\right]}^T$

Wherein, q=[q ₀q ₁q ₂q ₃] ^trepresent attitude four element, ω _bias=[ω _{r_bias}ω _{p_bias}ω _{y_bias}] ^tbe expressed as gyro zero evaluated error partially;

(3) define system state-transition matrix is:

$\mathrm{\Φ}=\frac{\∂f\left(X\right)}{\∂X}$

Wherein, $f\left(X\right)=\left[\begin{array}{c}-0.5(x_2{\mathrm{\ω}}_{\mathrm{bx}}+x_3{\mathrm{\ω}}_{\mathrm{by}}+x_4{\mathrm{\ω}}_{\mathrm{bz}})\\ 0.5(x_1{\mathrm{\ω}}_{\mathrm{bx}}-x_4{\mathrm{\ω}}_{\mathrm{by}}+x_3{\mathrm{\ω}}_{\mathrm{bz}})\\ 0.5(x_4{\mathrm{\ω}}_{\mathrm{bx}}+x_1{\mathrm{\ω}}_{\mathrm{by}}-x_3{\mathrm{\ω}}_{\mathrm{bz}})\\ -0.5(x_3{\mathrm{\ω}}_{\mathrm{bx}}-x_2{\mathrm{\ω}}_{\mathrm{by}}-x_1{\mathrm{\ω}}_{\mathrm{bz}})\\ 0\\ 0\\ 0\end{array}\right]$

Observation equation is set up by controller:

Z=h (X)+v, in this observation equation, Z=[φ θ ψ] ^tfor measuring variable, measurement noise v is the zero mean Gaussian white noise sequence of random independent, i.e. v ~ N (0, R);

Progressive one calculates system measurements transition matrix is:

$H=\frac{\∂h\left(X\right)}{\∂X};$

Two parts are divided into: a part utilizes the weight component of MEMS triaxial accelerometer sensitivity to solve attitude angle by corresponding for observed quantity, course angle information is estimated, respectively the measurement transition matrix H of corresponding two different computation periods after the course angle error obtained of another part trace information and gyro carry out PI correction ₁and H ₂;

Measure transition matrix H ₁process as follows:

The specific force f that what MEMS triaxial accelerometer recorded is suffered by carrier, if in inertial measurement cluster after de-noising to add table measured value be f _x, f _y, f _zwhen carrier is in static and quasistatic, because bearer rate is lower, the impact of Corioli's acceleration and other disturbing accelerations can be ignored, be " east northeast ground " at navigation coordinate, carrier coordinate system is in " under front right " situation, can be obtained the attitude angle of carrier by the measured value adding table according to the following formula;

φ＝-atan(f _y/f _z)，

$\mathrm{\θ}=\arcsin (f_x/\sqrt{f_x^2+f_y^2+f_z^2}),$

Corresponding measurement transition matrix is: $H_1={\left[\begin{array}{ccc}\frac{\∂\mathrm{\φ}}{\∂X}& \frac{\∂\mathrm{\θ}}{\∂X}& 0_{7\×1}\end{array}\right]}^T;$

Measure transition matrix H ₂process as follows:

Adopt trace information to come the course of regular calibration system, trace information can obtain the sensing deviation angle of antenna beam after treatment, by obtaining course angle error after conversion; Course angle error goes to correct gyro value ω after a PI controller _bz, can course angle ψ be obtained by after the gyro value integration after correction; Wherein, the gain of controller, K _pand K _iwith cutoff frequency ω and damping relevant:

K _i＝ω ²

Using course angle ψ also as the measurement variable of Kalman, can obtain measuring transition matrix:

$H_2={\left[\begin{array}{ccc}\frac{\∂\mathrm{\φ}}{\∂X}& \frac{\∂\mathrm{\θ}}{\∂X}& \frac{\∂\mathrm{\ψ}}{\∂X}\end{array}\right]}^T;$

(4) carrier movement condition discrimination;

Will speed up angle value and be converted to navigational coordinate system, according to rule judgment below:

If (| a _nx| > δ _x) | (| a _ny| > δ _y) | (| a _nz-g| > δ _z), assert to there is external acceleration, in formula, a _nx, a _nyand a _nzfor the accekeration under navigational coordinate system;

When carrier exists external acceleration, measure covariance matrix R change filter gain by adjustment, now, surveying covariance matrix is:

$R=\left[\begin{array}{ccc}1000\×r_{\mathrm{\φ}}& 0& 0\\ 0& 1000\×r_{\mathrm{\θ}}& 0\\ 0& 0& r_{\mathrm{\ψ}}\end{array}\right].$

2. antenna for satellite communication in motion low cost according to claim 1 follows the tracks of anti-interference method, and it is characterized in that, the design of wave filter is:

(1) computer card Germania yield value:

$K_k=P_k^{-}{H}_{\mathrm{ki}}^T{(H_{\mathrm{ki}}{P}_k^{-}{H}_{\mathrm{ki}}^T+R_k)}^{-1};$

(2) estimated value is upgraded:

${\hat{X}}_k={\hat{X}}_k^{-}+K_k(Z_{\mathrm{ki}}-h_i\left({\hat{X}}_k^{-}\right)),$

(3) matrix of error is upgraded:

$P_k=(I-K_k{H}_{\mathrm{ki}})P_k^{-},$

(4) prediction reference state error:

${\hat{X}}_{k+1}^{-}=f\left({\hat{X}}_k\right),$

(5) estimation error matrix:

$P_{k+1}^{-}={\mathrm{\Φ}}_k{P}_k{\mathrm{\Φ}}_k^T+Q,$

Wherein, when trace information is invalid, measurement matrix is H ₁, measured value corresponds to Z ₁, measuring estimated value is when trace information is effective, measurement matrix is H ₂, measured value corresponds to Z ₂, measuring estimated value is