CN1987355A - Earth satellite self astronomical navigation method based on self adaptive expansion kalman filtering - Google Patents

Abstract

The method includes steps: building up kinematical equation of earth satellite orbit based on rectangular coordinate system; building up measurement equation based on angular distance of starlight as measuring quantity; building up discrete type extended Kalman filtering equation to determine whether extended Kalman filtering is divergent; using prognostic filtering residual to determine whether extended Kalman filter is divergent; if yes, then the method carries out estimating statistic characteristics of noise; otherwise, carrying out calculation according to standard extended Kalman filtering program. The invention solves issue that divergence of noise filter caused by inaccurate determination of statistic characteristics of noise influences on navigation accuracy. Advantages are: independent, flexible and simple, and high precision. The invention is especially suitable to earth application satellite needed high navigation accuracy in resource, weather areas etc.

Description

A kind of earth satellite celestial self-navigation method based on adaptive extended kalman filtering

Technical field

The present invention relates to a kind of earth satellite celestial self-navigation method, can be used for earth applied satellite independent navigation location such as resource, communication, scouting, meteorology based on adaptive extended kalman filtering.

Background technology

Celestial navigation system is a kind ofly not carry out information transmission and exchange with the external world, does not rely on the complete autonomous navigational system of uphole equipment, is that a kind of celestial body information of utilizing optical sensor to record is carried out a kind of positioning navigation method that carrier positions is calculated.Its ultimate principle is to utilize the celestial body measurement information in conjunction with the dynamics of orbits equation, utilizes the navigation informations such as position and speed of optimal estimation method estimation space aircraft.That is to say that astronomical navigation method can be divided into three parts, the one, the accurate modeling of dynamics of orbits, the 2nd, the selection of measurement amount, the 3rd, the design of filtering (estimation) method.

Celestial navigation based on the dynamics of orbits equation comprises directly gentle sensitively two kinds of navigate modes that utilize starlight to reflect indirect responsive Horizon.Indirect responsive Horizon requires necessary fixed star generation atmospheric refraction, the starlight refraction angle that records as measurement information, this navigate mode requires to set up more accurate funtcional relationship between starlight refraction angle and the atmospheric density, and in fact is difficult to obtain the accurate model of atmospheric density.

Direct responsive Horizon is based on the another kind of autonomous astronomical navigation mode of dynamics of orbits, its principle is to utilize the tangential direction at the responsive earth of infrared earth instrument edge, and then obtain the line direction in earth satellite and the earth's core, utilizing star sensor to record the starlight direction vector of navigation fixed star, the vector of the earth's core direction vector and fixed star constitutes the starlight angular distance as the measurement amount, as shown in Figure 2.That this navigate mode has is simple in structure, with low cost, reliable, technology maturation and be easy to characteristics such as realization.

In satellite navigation is estimated, the normal at present optimal estimation method that adopts is a kalman filter method, this method hypothesis is carried out under a kind of ideal conditions, promptly requiring system model is linearity or small nonlinearity, system noise and measurement noise statistical property are the white Gaussian noise of zero-mean, and actual satellite navigation system not only system model have strong nonlinearity, system noise and measurement noise also are difficult to meet the demands, obviously the hypothesis prerequisite of kalman filter method is very harsh in practical engineering application, there is the defective that self can't overcome, these all are the main causes of dispersing that causes wave filter, filter divergence will cause navigation accuracy to descend, serious can't export correct navigation results, therefore say traditional based on kalman filter method exist self be difficult to overcome cause filter divergence, the defective that navigation accuracy descends because of Noise Estimation is inaccurate.

Summary of the invention

The technical matters that the present invention solves is: overcome that supposing the system noise and measurement noise are white Gaussian noise in traditional EKF method, cause the low deficiency of navigational system precision, a kind of earth satellite celestial self-navigation method based on adaptive extended kalman filtering is proposed, mode by direct responsive Horizon realizes the earth satellite autonomous astronomical navigation, the lastest imformation that at every turn calculates according to adaptive extended kalman filtering, derive the system noise and the measurement noise statistical property of realistic model, overcome because of noise statistics and determined the inaccurate noise filter divergence problem that causes, improved the earth navigation system accuracy and the scope of application greatly.

Technical scheme of the present invention is: a kind of earth satellite celestial self-navigation method based on adaptive extended kalman filtering, its feature is a measurement information with the starlight angular distance, dynamics of orbits equation in conjunction with earth satellite, utilize the adaptive extended kalman filtering method to obtain high-precision position, velocity estimation, at first judge by the size of EKF method calculation of filtered residual error whether wave filter is dispersed, and then startup adaptive extended kalman filtering program, utilize the maximum posteriori estimator to extrapolate the statistical property of system noise and measurement noise, greatly degree has overcome traditional Kalman filter causes filter divergence when disposal system noise and measurement noise are non-white Gaussian noise problem, further improves the navigation accuracy of earth satellite.

Concrete steps are as follows:

(1) set up earth satellite orbital motion equation based on rectangular coordinate system, i.e. state equation, by separate the differential equation calculate earth satellite the position (x, y, z) and speed (v _x, v _v, v _z) information;

(2) setting up with the starlight angular distance is the measurement equation of measurement amount:

(3) set up discrete type EKF equation,, derive the recurrence equation that discreteness is promoted Kalman filtering according to the minimum variance estimate principle with linearization again after state equation and the measurement equation discretize;

(4) judge whether EKF disperses, and utilizes the predictive filtering residual error is judged whether extended Kalman filter is dispersed, if satisfy the estimation that divergence case then carries out noise statistics, otherwise calculates according to the EKF program of standard;

(5) to the estimation of noise statistics in the earth satellite navigation system, comprise the system noise of earth satellite navigational system state model and the measurement noise statistical property estimation of measurement equation, adopt EKF according to the lastest imformation that calculates at every turn, extrapolate system noise and measurement noise statistical property in the real system, promptly utilize the approximate smooth estimated value that replaces of wave filter estimated value and predicted value, can obtain the maximum posteriori estimated value of suboptimum, and make EKF become optimum;

(6), be output as earth satellite state vector estimated value according to above-mentioned steps (1)～(5)

And variance matrix P, wherein state estimation value Comprise earth satellite position and velocity [x, y, z, v _x, v _y, v _z] ^T, state estimation variance matrix P comprises earth satellite position and velocity estimation variance [P _x, P _y, P _z, P _Vx, P _Vy, P _Vz] ^T

Ultimate principle of the present invention is: utilize star sensor observation navigation fixed star to obtain the direction of this starlight at star sensitive measurement coordinate system, by the conversion that star sensor is installed matrix, can be regarded as the direction of starlight in the earth satellite body coordinate system; Utilize infrared earth sensor directly to record tangential direction or the normal direction of earth satellite again, obtain the direction of the earth's core vector in the earth satellite body coordinate system to earth edge; Then obtain astronomical measurement information such as starlight angular distance, as shown in Figure 2 etc., can estimate the positional information of earth satellite again in conjunction with dynamics of orbits equation and advanced person's filtering technique.The method for adaptive kalman filtering principle is on the basis of EKF, the i.e. preceding k township lastest imformation that calculates according to each EKF, derive the system noise and the measurement noise statistical property of realistic model, solved in traditional EKF directly that supposing the system noise and measurement noise are the defective of white Gaussian noise, improved adaptive extended kalman filtering device convergence and earth satellite navigational system navigation and positioning accuracy.

The present invention's advantage compared with prior art is: solved that the legacy card Kalman Filtering is applied in the non-linear stronger earth satellite navigational system because supposing the system noise and amount side noise are the defective that non-white Gaussian noise caused, designed the adaptive extended kalman filtering device.The lastest imformation in k step is calculated the characteristics of the true statistical property of system noise and measurement noise before utilizing in the wave filter, solved traditional extended Kalman filter and estimated the inaccurate filter divergence that causes because of noise statistics, influence the problem of filtering accuracy, improved the navigation accuracy and the scope of application thereof of earth satellite, made it be applicable to earth satellites such as resource that navigator fix is had relatively high expectations, communication, scouting, meteorology more.

Description of drawings

Fig. 1 is a process flow diagram of the present invention;

Fig. 2 is the measurement information-starlight angular distance synoptic diagram among the present invention.

Embodiment

The concrete flow process of implementing of the present invention as shown in Figure 1, earlier the starlight angular distance that constitutes of the earth's core direction vector of measuring of starlight vector that is recorded by star sensor and horizon instrument is as the measurement amount, state equation in conjunction with earth satellite, utilize the EKF method to estimate whether the prediction variance satisfies the requirement of filter divergence, just utilize lastest imformation to calculate the statistical property of system noise and measurement noise, promptly utilize the approximate smooth estimated value that replaces of Filtering Estimation value and predicted value, obtain the secondary maximum posterior estimate, carry out EKF again, estimate the position of earth satellite, velocity information; Otherwise wave filter is not dispersed position, the velocity information that goes out earth satellite with regard to direct estimation.

Concrete implementation step is as follows.

1, produces the orbital data of nominal

1. coordinate system: equator, J2000.0 the earth's core inertial coordinates system

2. nominal orbit parameter is set, and purpose is the track of generation standard.

Semi-major axis: a=7136.635km

Excentricity: e=1.809 * 10 ^-3

Orbit inclination: i=65 °

Right ascension of ascending node: Ω=30.00 °

Nearly lift angle distance: ω=30.00 °

2, foundation is based on the earth satellite orbital motion equation of rectangular coordinate system

When the motion of research earth satellite, choose (J2000.0) geocentric equatorial polar coordinate epoch.At this moment, the satellite navigation system state model of selecting for use usually (dynamics of orbits model) is

$\left\{\begin{array}{c}\frac{\mathrm{dx}}{\mathrm{dt}}=v_x\\ \frac{\mathrm{dy}}{\mathrm{dt}}=v_y\\ \frac{\mathrm{dz}}{\mathrm{dt}}=v_z\\ \frac{d{v}_x}{\mathrm{dt}}=-\mathrm{\μ}\frac{x}{r^3}[1-J_2\left(\frac{R_e}{r}\right)(7.5\frac{z^2}{r^2}-1.5)]+\mathrm{\Δ}{F}_x\\ \frac{d{v}_y}{\mathrm{dt}}=-\mathrm{\μ}\frac{y}{r^3}[1-J_2\left(\frac{R_e}{r}\right)(7.5\frac{z^2}{r^2}-1.5)]+\mathrm{\Δ}{F}_y\\ \frac{d{v}_y}{\mathrm{dt}}=-\mathrm{\μ}\frac{z}{r^3}[1-J_2\left(\frac{R_e}{r}\right)(7.5\frac{z^2}{r^2}-4.5)]+\mathrm{\Δ}{F}_z\end{array}\right.---\left(6\right)$

In the formula, $r=\sqrt{x^2+y^2+z^2},$ Be abbreviated as X (t)=f (X, t)+w (t) (7)

In the formula, state vector X=[x y z v _xv _yv _z] ^T, x, y, z, v _x, v _y, v _zBe respectively position and the speed of satellite in X, Y, three directions of Z; μ is a geocentric gravitational constant; R is the satellite position parameter vector; J ₂Be the terrestrial gravitation coefficient; Δ F _x, Δ F _y, Δ F _zBe the high-order perturbing term of perturbation of earths gravitational field and the influence of day, month perturbation and perturbative forces such as solar radiation pressure perturbation and atmospheric perturbation, system noise w (t) expression is used in the influence of these perturbative forces usually in simplified model.

Above-mentioned equation is the continuous system state equation, and its discretize is expressed as

X _k+1＝Φ _k+1kX _k+W _k k＝1，2，3... (8)

In the formula, Φ _{K+1, k}Step transition matrix for the k moment to the k+1 moment; W _kBe the system noise matrix.

Have for the zero-mean white noise for system noise

E{W(k)}＝0，E{W(k)W ^T(j)}＝Q _kδ _kj j＝1，2，3... (9)

3, be that the measurement amount is set up measurement equation with the starlight angular distance

The starlight angular distance is a kind of measurement amount of often using in the celestial navigation, and starlight angular distance α refers to the direction vector of the navigation fixed star starlight that observes from satellite and the angle between the direction vector of the earth's core, as shown in Figure 2.From the geometric relationship shown in the figure, the expression formula and the corresponding measurement equation that can obtain starlight angular distance α are respectively

$\mathrm{\α}=\arccos (-\frac{r\·s}{r})---\left(10\right)$

$z(k+1)=h[X(k+1),k+1]+V(k+1)=\mathrm{\α}+V_{k+1}=\arccos (-\frac{r\·s}{r})+V_{k+1}---\left(11\right)$

In the formula, r is the position vector of satellite in the Earth central inertial spherical coordinate system, is obtained by the Horizon sensor; S is the unit vector of nautical star starlight direction, is discerned by star sensor.

Have for the zero-mean white noise for measurement noise

E{V(k)}＝0，E{V(k)V ^T(j)}＝R _kδ _kj (12)

4, set up discrete type EKF equation

The Kalman filtering of expansion be earlier with the Nonlinear Vector function  in the stochastic non-linear system model and h around the filter value linearization, obtain the system linearization model, application card Kalman Filtering fundamental equation then solves the nonlinear filtering problem.With the state equation of discrete random nonlinear system (8) and the Nonlinear Vector function  in the measurement equation (11) and h around filter value

Be launched into Taylor series, and omit above of secondary, respectively

$Z(k+1)=h[\hat{X}(k+1|k),k+1]+\frac{\∂h[\hat{X}(k+1),k+1]}{\∂X(k+1)}{|}_{X\left(k\right)=\hat{X}(k+1|k)}[X(k+1)-\hat{X}(k+1|k)]+V(k+1)---\left(14\right)$

The recurrence equation that discreteness is promoted Kalman filtering is:

K(k+1)＝P(k+1|k)H ^T(k+1)[H(k+1)P(k+1|k)H ^T(k+1)+R _k+1] ^-1 (16)

$P(k+1|k)=\mathrm{\Φ}[k+1,k]P\left(k\right|k){\mathrm{\Φ}}^T\left[k+1,k\right]+Q_k{\mathrm{\Γ}}^T[\hat{X}\left(k\right|k),k]---\left(17\right)$

P(k+1|k+1)＝[I-K(k+1)H(k+1)]P(k+1|k) (18)

Initial value is

$\hat{X}\left(0\right|0)=E\left\{X\left(0\right)\right\}={\mathrm{\μ}}_x\left(0\right);$

P(0|0)＝Var{X(0)}＝P _x(0)； (20)

In the formula,

$H(k+1)=\frac{\∂h[X(k+1),k+1]}{\∂X(k+1)}{|}_{X\left(k\right)=\hat{X}(k+1|k)}.$ $P={\left[P_x{P}_y{P}_z{P}_{v_x}{P}_{v_y}{P}_{v_z}\right]}^T$ Be state vector X estimation variance matrix.

5, judge whether EKF disperses, and utilizes the predictive filtering residual error is judged whether extended Kalman filter is dispersed, if satisfy the estimation that divergence case then carries out noise statistics, otherwise calculates according to the EKF program of standard.

Calculation of filtered residual error V _K/k=Z _k-H _kX _{K/ (k-1)}, with and variance $S_k=H_k{P}_{k/(k-1)}{H}_k^T+R_k,$ Judge whether divergence case is wave filter:

V _k/k ^TV _k/k≤t×Tr(S _k) (21)

In the formula, t is adjustability coefficients (t 〉=1), if do not satisfy condition, judges that then EKF disperses,, next step carries out the estimation of noise statistics in the earth satellite navigational system, i.e. estimating system noise and measurement noise statistical property.

6, to the estimation of noise statistics in the earth satellite navigation system, comprise the system noise of earth satellite navigational system state model and the measurement noise statistical property estimation of measurement equation, adopt EKF according to the lastest imformation that calculates at every turn, extrapolate system noise and measurement noise statistical property in the real system, promptly utilize the approximate smooth estimated value that replaces of wave filter estimated value and predicted value, can obtain the maximum posteriori estimated value of suboptimum, and make EKF become optimum.

1. the statistical property of system noise is:

In the formula, Be prediction residual,

Be system noise error mean and covariance.

2. the statistical property of measurement noise is:

In the formula, Be the measurement residual error,

Be measurement noise error mean and covariance.

7, the nominal trajectory that utilizes step 1 to produce utilizes step 2 to find the solution state equation, utilizes step 3 to set up measurement equation, in the discrete type EKF equation in the mutual relationship substitution step 4 between step 2 and the step 3, estimate earth satellite position and velocity [x, y, z, v _x, v _y, v _z] ^TAnd to pass through step 5 calculation of filtered residual error at estimation procedure, judge in view of the above whether extended Kalman filter is dispersed, if filter divergence is by the statistical property of step 6 estimation model noise, the statistical property that estimates again in the substitution step 4, is estimated earth satellite state vector estimated value And variance matrix P, wherein state estimation value

Comprise earth satellite position and velocity [x, y, z, v _x, v _y, v _z] ^T, state estimation variance matrix P comprises earth satellite position and velocity estimation variance [P _x, P _y, P _z, P _Vx, P _Vy, P _Vz] ^T

The content that is not described in detail in the instructions of the present invention belongs to this area professional and technical personnel's known prior art.

Claims (3)

1, a kind of earth satellite celestial self-navigation method based on adaptive extended kalman filtering is characterized in that comprising the following steps:

(1) set up earth satellite orbital motion equation based on rectangular coordinate system, i.e. state equation, by separate the differential equation calculate earth satellite the position (x, y, z) and speed (v _x, v _y, v _z) information;

(2) setting up with the starlight angular distance is the measurement equation of measurement amount;

(3) set up discrete type EKF equation,, derive the recurrence equation that discreteness is promoted Kalman filtering according to the minimum variance estimate principle with linearization again after state equation and the measurement equation discretize;

(4) judge whether EKF disperses, and utilizes the predictive filtering residual error is judged whether extended Kalman filter is dispersed, if satisfy the estimation that divergence case then carries out noise statistics, otherwise calculates according to the EKF program of standard;

(5) to the estimation of noise statistics in the earth satellite navigation system, comprise the system noise of earth satellite navigational system state model and the measurement noise statistical property estimation of measurement equation, adopt EKF according to the lastest imformation that calculates at every turn, extrapolate system noise and measurement noise statistical property in the real system, promptly utilize the approximate smooth estimated value that replaces of wave filter estimated value and predicted value, can obtain the maximum posteriori estimated value of suboptimum, and make EKF become optimum;

(6), be output as earth satellite state vector estimated value according to above-mentioned steps (1)～(5) And variance matrix P, wherein state estimation value

Comprise earth satellite position and velocity [x, y, z, v _x, v _y, v _z] ^T, state estimation variance matrix P comprises earth satellite position and velocity estimation variance [P _x, P _y, P _z, P _Vx, P _Vy, P _Vz] ^T

2, a kind of earth satellite celestial self-navigation method based on adaptive extended kalman filtering according to claim 1 is characterized in that: whether the EKF in the described step (4) disperses is judged to be calculation of filtered residual error V _K/k=Z _k-H _kX _{K/ (k-1)}, with and variance $S_k=H_k{P}_{k/(k-1)}{H}_k^T+R_k,$ Judge whether divergence case is wave filter:

${V_{k/k}}^T{V}_{k/k}\≤t\×\mathrm{Tr}\left(S_k\right)---\left(1\right)$

In the formula, t is adjustability coefficients (t 〉=1), X _k, Z _k, H _k, V _kDifference k quantity of state constantly, the measurement amount measures matrix and measures residual error; S _k, P _k, R _kBe respectively the variance of filtering residual error, estimate square error, the covariance of measurement noise, Tr is for asking S _kMatrix trace if do not satisfy condition, judges that then the legacy card Kalman Filtering disperses, and starts the estimation to noise statistics in the earth satellite navigation system, respectively estimating system noise and measurement noise statistical property.

3, a kind of earth satellite celestial self-navigation method according to claim 1 based on adaptive extended kalman filtering, it is characterized in that: in the described step (5) to the estimation of system noise and measurement noise statistical property, employing is calculated system noise and measurement noise characteristic according to EKF according to the lastest imformation that calculates at every turn, promptly utilizes the Filtering Estimation value

And predicted value

The approximate smooth estimated value that replaces obtains the secondary maximum posterior estimate, and the concrete steps of this method are:

1. the statistical property of system noise is:

${\hat{q}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{X}_j^{\%}---\left(2\right)$

${\hat{Q}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}(X_j^{\%}-{\hat{q}}_j){(X_j^{\%}-{\hat{q}}_j)}^T---\left(3\right)$

In the formula, $X_j^{\%}={\hat{X}}_j-{\hat{X}}_{j/(j-1)}={\hat{X}}_j-{\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{j-1}$ Be prediction residual,

Be system noise error mean and covariance, Φ _{K, k-1}Be t _K-1A step transition matrix constantly;

2. the statistical property of measurement noise is:

${\hat{r}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}{Z}_j^{\%}---\left(4\right)$

${\hat{R}}_k=\frac{1}{k}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}(Z_j^{\%}-{\hat{r}}_j){(Z_j^{\%}-{\hat{r}}_j)}^T---\left(5\right)$

In the formula, $Z_j^{\%}=Z_j-H_k{\hat{X}}_j$ Be the measurement residual error,

Be measurement noise error mean and covariance.