CN103294931A - System state estimation method based on improved nonlinear robust filtering algorithm - Google Patents

Abstract

The invention discloses a system state estimation method based on an improved nonlinear robust filtering algorithm and relates to the technical field of space navigation. The method is based on a Huber nonlinear regression algorithm, so that high filtering accuracy and stable filtering output can be obtained under the condition that a measuring output equation is nonlinear, and measuring noises are in non-Gaussian distribution.

Description

System state estimation method based on improved non linear robust filtering algorithm

Technical field

What the present invention relates to is a kind of method of space navigation technical field, specifically be a kind of for fields such as navigation, target following, fault detects, particularly be applied to the distributed spacecraft system state estimation method based on improved non linear robust filtering algorithm of navigation relatively.

Background technology

In the relative navigation application of distributed spacecraft, the most frequently used navigation algorithm is Kalman filtering.When the spacecraft relative position is far away, generally adopt GPS to carry out relative position measurement.Yet when the spacecraft relative position was nearer, for example, super close distance formation flight or two Spacecraft Rendezvous butt joint stage, the reliability of GPS can not satisfy the demand that relative navigation is measured, and must switch to radar or laser measurement system.According to pertinent literature as can be known, there is non-Gauss measurement noise in this class measuring system, so must adopt the nonlinear filtering algorithm of robust to carry out relative Navigation Filter design.In addition, in order to obtain easy easy-to-use filtering recurrence equation, the statistical property of Kalman filtering hypothesis process and measurement noise satisfies Gaussian distribution.Mostly the noisiness of measuring system all is non-Gauss, so design robust nonlinear filtering algorithm is significant but in actual applications.

Find by prior art documents, Karlgaard.C.D, write articles the intersection navigation that has robustness on the Robust Rendezvous Navigation in Elliptical Orbit(elliptical orbit) [J] //JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS Vol.29, No.2, March-April2006. " elliptical orbit robust intersection navigation ", proposed a kind of robust nonlinear filtering algorithm based on the Huber linear regression in the literary composition, and obtained than the good navigation results of the non-robust Kalman of tradition algorithm.The algorithm that proposes in the literary composition has it to use singularity, when carrying out the spacecraft relative position measurement, is linear owing to measure output equation, so the linear regression algorithm that proposes in the literary composition can be applied directly in the relative Navigation Filter design.But in nonlinear measurement output is used, need carry out linearization to output equation, and when recursion is measured the prediction output matrix, use the Jacobian matrix of measuring output function, can reduce filtering output result's precision like this or cause filtering divergence.Therefore, need traditional non-linear Kalman filtering device be redesigned, make to have higher filtering accuracy and metastable filtering output.

Summary of the invention

The present invention is directed to the prior art above shortcomings, a kind of system state estimation method based on improved non linear robust filtering algorithm is proposed, make measure output equation be non-linear, to measure noise be under the situation of non-Gaussian distribution, obtain higher filtering accuracy and more stable filtering output, can be applicable to fields such as navigation, target following and fault detect.

The present invention is achieved by the following technical solutions, the present invention includes following steps:

At nonlinear system: $\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ y_k=g\left(x_k\right)+v_k\end{array},$ It is as follows to the present invention includes step:

The first step, k=1, the initialization system state variable

With system state error variance battle array

Second step, structure nonlinear regression model (NLRM): $\left[\begin{array}{c}{y}_k\\ {\stackrel{\‾}{x}}_k\end{array}\right]=\left[\begin{array}{c}g\left(x_k\right)\\ x_k\end{array}\right]+\left[\begin{array}{c}{v}_k\\ {-\mathrm{\δ}}_k\end{array}\right],$ Wherein:

Be a step status predication of EKF, can obtain δ by EKF one-step prediction equation recursion _kBe time of day and predicted state

Between error; If $\mathrm{\&zeta;}=\left[\begin{array}{c}{v}_k\\ {-\mathrm{\&delta;}}_k\end{array}\right]$ And satisfy: $E(\mathrm{\&zeta;}\&CenterDot;{\mathrm{\&zeta;}}^T)=\left[\begin{array}{cc}{\mathrm{<figure-callout\; id="0"\; label="R\; k"\; filenames="HDA0000343035980000012.png"\; state="\{\{state\}\}">R</figure-callout>}}_k& 0\\ 0& {\stackrel{\&OverBar;}{P}}_k\end{array}\right]=S_k^T\&CenterDot;S_k,$ Wherein: R _kBe main Gauss measurement noise variance matrix,

It is status predication error variance battle array.

The 3rd goes on foot, the both sides of nonlinear regression model (NLRM) be multiply by simultaneously

Nonlinear regression problem can be converted to z _k=h (x _k)+ξ _k, wherein: $h\left(x_k\right)=S_k^{-1}{\left[\begin{array}{cc}g\left(x_k\right)& x_k\end{array}\right]}^T,$ ${\mathrm{\&xi;}}_k=S_k^{-1}\mathrm{\&zeta;}$ With $z_k=S_k^{-1}{\left[\begin{array}{cc}{y}_k& {\stackrel{\&OverBar;}{x}}_k\end{array}\right]}^T;$

The definition cost function:

Wherein: e _iBe residual vector e=z _k-h (x _k) i element, m is the dimension of vectorial e;

The Huber evaluation function is $\mathrm{\&rho;}\left(e_i\right)=\left\{\begin{array}{cc}\frac{1}{2}{e}_i^2& \left|e_i\right|<\mathrm{\&gamma;}\\ \mathrm{\&gamma;}\left|e_i\right|-\frac{1}{2}{\mathrm{\&gamma;}}^2& \left|e_i\right|\&GreaterEqual;\mathrm{\&gamma;}\end{array}\right.,$ To getting after the cost function differentiate

Wherein: γ is for regulating parameter.

Definition

Ψ=diag[ψ (e _i)], then to obtaining after the cost function differentiate:

Wherein: M _kBe h (x _k) Jacobian matrix, launch the back and find the solution with least square method, its initial value is made as $x_k^{\left(0\right)}={\left(M_k^T{M}_k\right)}^{-1}{M}_k^T{z}_k,$ Its iteration convergence solution is $x_k^{(j+1)}={\left(M_k^T{\mathrm{\&Psi;}}^{\left(j\right)}{M}_k\right)}^{-1}{M}_k^T{\mathrm{\&Psi;}}^{\left(j\right)}{z}_k,$ Wherein: j is iterations.When asking for the iteration convergence solution, need to choose corresponding iteration convergence threshold value by concrete application example.

The 4th goes on foot, will be decomposed into according to status predication and measurement prediction residual by the final matrix Ψ that iterative state is estimated $\mathrm{\&Psi;}=\left[\begin{array}{cc}{\mathrm{\&Psi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA0000343035980000012.png"\; state="\{\{state\}\}">y</figure-callout>}}& 0\\ 0& {\mathrm{\&Psi;}}_x\end{array}\right],$ Then robust Kalman filtering gain matrix is:

$K_k={\stackrel{\&OverBar;}{P}}_k^{1/2}{\mathrm{\&Psi;}}_x^{-1}{\stackrel{\&OverBar;}{P}}_k^{T/2}{H}_k{(H_k{\stackrel{\&OverBar;}{P}}_k^{1/2}{\mathrm{\&Psi;}}_x^{-1}{\stackrel{\&OverBar;}{P}}_k^{T/2}{H}_k^T+R_k^{1/2}{\mathrm{\&Psi;}}_y^{-1}{R}_k^{T/2})}^{-1},$ Wherein: H _kBe to measure the output equation Jacobian matrix, measure the system state after upgrading, namely by vector The system state that refers to, obtain by standard card Kalman Filtering recurrence equation, State error variance battle array is measured the renewal matrix: ${\hat{P}}_k=(I-K_k{H}_k){\stackrel{\&OverBar;}{P}}_k^{1/2}{\mathrm{\&Psi;}}^{-1}{\stackrel{\&OverBar;}{P}}_k^{T/2}.$

The 5th step, k=k+1, and turn back to state estimation that the first step is carried out next moment.

Technique effect

The present invention's advantage compared with prior art is: in some practical engineering application, the noisiness of measuring system is non-Gauss, adopts method of the present invention than traditional Kalman filtering algorithm non-linear system status to be estimated to obtain the estimated accuracy of speed of convergence and Geng Gao faster.

Description of drawings

Fig. 1 is the performance comparison diagram of algorithm position estimation error among the embodiment.

Fig. 2 is the performance comparison diagram of algorithm speed evaluated error among the embodiment.

Fig. 3 is the performance comparison diagram of algorithm ballistic coefficient evaluated error among the embodiment.

Embodiment

Below embodiments of the invention are elaborated, present embodiment is being to implement under the prerequisite with the technical solution of the present invention, provided detailed embodiment and concrete operating process, but protection scope of the present invention is not limited to following embodiment.

Embodiment 1

Present embodiment carries out the real-time system state estimation for utilizing radargrammetry.

Dynamic system model is: $\begin{array}{c}{\stackrel{.}{x}}_1=-x_2\\ {\stackrel{.}{x}}_2={-e}^{-\mathrm{\&eta;}{x}_1}{x}_2^2{x}_3^2\\ {\stackrel{.}{x}}_3=0\end{array},$ Wherein: x ₁Be object height, x ₂Be the object falling speed, x ₃It is constant trajectory parameter.

The radargrammetry equation:

Wherein: a is the height of radar station, and b is the horizontal range between object and the radar, v _kBe the measurement noise of zero-mean, and have following mixing probability density function:

$p\left(v_k\right)=\frac{1-\mathrm{\&epsiv;}}{{\mathrm{\&sigma;}}_1\sqrt{2\mathrm{\&pi;}}}\exp [-0.5{(v_k/{\mathrm{\&sigma;}}_1)}^2]+\frac{\mathrm{\&epsiv;}}{{\mathrm{\&sigma;}}_2\sqrt{2\mathrm{\&pi;}}}\exp [-0.5{(v_k/{\mathrm{\&sigma;}}_2)}^2],$ Wherein: σ ₁And σ ₂Be the standard deviation of Gaussian noise separately, ε is contamination factor and the pollution ratio that represents error model, in the present embodiment, gets σ ₂=5 σ ₁As can be seen from the above equation, the statistical property of measurement noise is non-Gauss.

Simulation parameter

Starting condition

The first step, initialization, k=1, concrete parameter setting is provided by the starting condition form.

Second step, structure nonlinear regression model (NLRM): $\left[\begin{array}{c}{y}_k\\ {\stackrel{\&OverBar;}{x}}_k\end{array}\right]=\left[\begin{array}{c}g\left(x_k\right)\\ x_k\end{array}\right]+\left[\begin{array}{c}{v}_k\\ {-\mathrm{\&delta;}}_k\end{array}\right];$

The 3rd goes on foot, tries to achieve S _k, the nonlinear regression model (NLRM) both sides be multiply by simultaneously

Obtain following nonlinear regression problem: z _k=h (x _k)+ξ _k

According to the Huber evaluation function, can get cost function:

The 4th the step, by cost function is carried out differentiate, be converted into matrix equality then, and then utilize least square method to ask its iteration convergence solution, getting the iteration convergence threshold value in iterative process is 1e ^-6, can obtain final Huber matrix Ψ, according to measuring prediction and status predication residual error with matrix decomposition be $\mathrm{\&Psi;}=\left[\begin{array}{cc}{\mathrm{\&Psi;}}_{\mathrm{<figure-callout\; id="0"\; label="y"\; filenames="HDA0000343035980000012.png"\; state="\{\{state\}\}">y</figure-callout>}}& 0\\ 0& {\mathrm{\&Psi;}}_x\end{array}\right].$

Calculating robust Kalman filtering gain matrix is:

$K_k={\stackrel{\&OverBar;}{P}}_k^{1/2}{\mathrm{\&Psi;}}_x^{-1}{\stackrel{\&OverBar;}{P}}_k^{T/2}{H}_k{(H_k{\stackrel{\&OverBar;}{P}}_k^{1/2}{\mathrm{\&Psi;}}_x^{-1}{\stackrel{\&OverBar;}{P}}_k^{T/2}{H}_k^T+R_k^{1/2}{\mathrm{\&Psi;}}_y^{-1}{R}_k^{T/2})}^{-1};$

Measure the state after upgrading: ${\hat{x}}_k={\stackrel{\&OverBar;}{x}}_k+K_k(y_k-{\stackrel{\&OverBar;}{y}}_k);$

State error variance battle array is measured the renewal matrix:

The 5th step, k=k+1 turn back to the first step and carry out next system state estimation constantly.

As shown in Figure 1-Figure 3, be present embodiment effect synoptic diagram, with reference to the accompanying drawings as seen, this method can obtain the estimated accuracy of speed of convergence and Geng Gao faster.

Claims (2)

1. the system state estimation method based on improved non linear robust filtering method is characterized in that, may further comprise the steps:

The first step, k=1, the initialization system state variable

With system state error variance battle array

Second step, structure nonlinear regression model (NLRM): $\left[\begin{array}{c}{y}_k\\ {\stackrel{\&OverBar;}{x}}_k\end{array}\right]=\left[\begin{array}{c}g\left(x_k\right)\\ x_k\end{array}\right]+\left[\begin{array}{c}{v}_k\\ -{\mathrm{\&delta;}}_k\end{array}\right],$ Wherein:

Be a step status predication of EKF, can obtain δ by EKF one-step prediction equation recursion _kBe time of day and predicted state

Between error; If $\mathrm{\&zeta;}=\left[\begin{array}{c}{v}_k\\ {-\mathrm{\&delta;}}_k\end{array}\right]$ And satisfy: $E(\mathrm{\&zeta;}\&CenterDot;{\mathrm{\&zeta;}}^T)=\left[\begin{array}{cc}{R}_k& 0\\ 0& {\stackrel{\&OverBar;}{P}}_k\end{array}\right]=S_k^T\&CenterDot;S_k,$ Wherein: R _kBe main Gauss measurement noise variance matrix,

It is status predication error variance battle array;

The 3rd goes on foot, the both sides of nonlinear regression model (NLRM) be multiply by simultaneously

Nonlinear regression problem can be converted to z _k=h (x _k)+ξ _k, wherein: $h\left(x_k\right)=S_k^{-1}{\left[\begin{array}{cc}g\left(x_k\right)& x_k\end{array}\right]}^T,$ ${\mathrm{\&xi;}}_k=S_k^{-1}\mathrm{\&zeta;}$ With $z_k=S_k^{-1}{\left[\begin{array}{cc}{y}_k& {\stackrel{\&OverBar;}{x}}_k\end{array}\right]}^T;$

The definition cost function:

Wherein: e _iBe residual vector e=z _k-h (x _k) i element, m is the dimension of vectorial e;

The Huber evaluation function is $\mathrm{\&rho;}\left(e_i\right)=\left\{\begin{array}{cc}\frac{1}{2}{e}_i^2& \left|e_i\right|<\mathrm{\&gamma;}\\ \mathrm{\&gamma;}\left|e_i\right|-\frac{1}{2}{\mathrm{\&gamma;}}^2& \left|e_i\right|\&GreaterEqual;\mathrm{\&gamma;}\end{array}\right.,$ To getting after the cost function differentiate

Wherein: γ is for regulating parameter;

Definition Ψ=diag[ψ (e _i)], then to obtaining after the cost function differentiate:

Wherein: M _kBe h (x _k) Jacobian matrix, launch the back and find the solution with least square method, its initial value is made as $x_k^{\left(0\right)}={\left(M_k^T{M}_k\right)}^{-1}{M}_k^T{z}_k,$ Its iteration convergence solution is $x_k^{(j+1)}={\left(M_k^T{\mathrm{\&Psi;}}^{\left(j\right)}{M}_k\right)}^{-1}{M}_k^T{\mathrm{\&Psi;}}^{\left(j\right)}{z}_k,$ Wherein: j is iterations, when asking for the iteration convergence solution, needs to choose corresponding iteration convergence threshold value by concrete application example;

The 4th goes on foot, will be decomposed into according to status predication and measurement prediction residual by the final matrix Ψ that iterative state is estimated $\mathrm{\&Psi;}=\left[\begin{array}{cc}{\mathrm{\&Psi;}}_y& 0\\ 0& {\mathrm{\&Psi;}}_x\end{array}\right],$ Then robust Kalman filtering gain matrix is:

$K_k={\stackrel{\&OverBar;}{P}}_k^{1/2}{\mathrm{\&Psi;}}_x^{-1}{\stackrel{\&OverBar;}{P}}_k^{T/2}{H}_k{(H_k{\stackrel{\&OverBar;}{P}}_k^{1/2}{\mathrm{\&Psi;}}_x^{-1}{\stackrel{\&OverBar;}{P}}_k^{T/2}{H}_k^T+R_k^{1/2}{\mathrm{\&Psi;}}_y^{-1}{R}_k^{T/2})}^{-1},$ Wherein: H _kBe to measure the output equation Jacobian matrix, measure the system state after upgrading, namely by vector

The system state that refers to, obtain by standard card Kalman Filtering recurrence equation, ${\hat{x}}_k={\stackrel{\&OverBar;}{x}}_k+K_k(y_k-{\stackrel{\&OverBar;}{y}}_k);$ State error variance battle array is measured the renewal matrix: ${\hat{P}}_k=(I-K_k{H}_k){\stackrel{\&OverBar;}{P}}_k^{1/2}{\mathrm{\&Psi;}}^{-1}{\stackrel{\&OverBar;}{P}}_k^{T/2};$

The 5th step, k=k+1, and turn back to state estimation that the first step is carried out next moment.

2. method according to claim 1 is characterized in that, described nonlinear system is $\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+w_{k-1}\\ y_k=g\left(x_k\right)+v_k\end{array}.$

Images