2.1. Strap-down Inertial Navigation System/Global Navigation Satellite System (SINS/GNSS) Integrated Navigation State Model
For a high-speed UAV system, the state vector of navigation is defined as X
, whose elements include INS error and inertial sensors’ error.
The navigation system is based on the East-North-Up (ENU) frame, and is the error components of attitude; represents the error components of velocity in the ENU frame; represents the error components of latitude, longitude and height; represents the drift of gyroscope in body frame; represents the bias of accelerometer in body frame. The superscripts b, n respectively represent body frame and ENU frame. T represents the transposition of the matrix.
The state equation is given as follows,
where
represents the measurement noise of gyroscope and accelerometer;
(
) represents the transition matrix;
represents the noise exciting matrix.
SINS/GNSS integrated system uses loosely coupled closed-loop architecture, and the measurement equation is given as follows,
where
represents measurement matrix;
, and
and
represent the velocity and position’s measurement noise of satellite receiver;
and
respectively represent the velocity obtained by INS and GNSS;
and
respectively represent the position obtained by INS and GNSS.
2.2. Algorithm Designs
Under the condition that the system model and noise characteristic are already known, KF is linear optimal. The SHAKF breaks the constraint that Q-matrix and R-matrix must be known beforehand, and realizes the on-line estimation of system and measurement noise. This method puts more weights on the residual, and can improve the tracking ability in a high-dynamic environment. However, if practical measurement noise is lower than theoretical one or the priori estimation error is too large, R-matrix may lose its positive definiteness, so that the filtering results will diverge. By using fading factor when calculating P-matrix, STF reduces the weights of the prior knowledge and historical information in current state estimation. Therefore, this method is a good way to track sudden changes. The basic assumption of STF, like KF, is that the noise characteristic obeys or approximately obeys Gaussian distribution. However, the noise under the high-dynamic flight condition of UAV cannot be described as Gaussian distribution. For example, there may be hopping points in GNSS data, thus a large estimation error will occur by then. The H∞ filter can guarantee the system’s availability under the condition of strong interference, but its average accuracy decreases. The proposed method of integrated filtering, strong adaptive Kalman filter, combines the three methods mentioned above. Based on the framework of KF, SAKF adds adaptive estimation step for error covariance matrices, and R-matrix is considered as the criterion whether the filtering process is abnormal or not. In abnormal condition, the algorithm will choose H∞ filter as the filtering main body. Otherwise, STF will be chosen.
Figure 1 shows the process of SAKF. The whole process includes 4 parts.
Part 1. State prediction update equation is given as follows,
is the priori state estimation at step
k.
is the estimation from time step
k − 1 to
k.
relates the state at time step
k to the state at step
k + 1.
Part 2. Residual update equation is given as follows,
is the residual at step
k,
is the measurement value at step
k, and
represent the measurement matrix.
Part 3. Measurement of anomaly detection step
The adaptive estimation method for the Q-matrix and R-matrix does not conflict with the framework of STF and H∞ filter, but the intermediate steps of STF and H∞ filter are conflicts with each other. So it is necessary to judge whether the measurement information is abnormal. The algorithm can use STF under normal condition, or switch to the H∞ filter under abnormal conditions.
Firstly, we should set the upper bound of R-matrix, and the matrix of maximum values named
is given as follows,
where
is the number of measurement vectors. Before each update, a detection should be made to check whether the estimated value of the
vector in R-matrix is smaller than
(
). The inequality is given as follows, whose result is considered as the criterion of anomaly or divergence. Here, the superscript
means this variable corresponding the
vector in R-matrix.
When every
satisfies the condition of inequality (7), we believe the measurement results are usable and the filter’s work state is stable. So the R-matrix update equation can be showed as follows,
Then update the measurement information using the updated equation of STF, which is given as follows:
Here
represents the diagonal matrix of fading factors, which is given as follows,
Each fading factor can be gotten by Equation (11).
where
is a fixed parameter, and the value of
can be obtained by Equations (12)–(16).
where the initial value of
is set as the covariance matrix of residual. Then the forgetting factor
adjusts the ratio between
and
in the following iterations. As
,
. This indicates that a lager weight is put on the current information than historical information. Therefore, the residual plays an important role in the filtering process, and the tracking ability can be improved in this way.
If there exists
which does not satisfy
, then the elements of R-matrix are updated as follows,
Then the updated equation of H
∞ filter is used as the filtering step:
Set constrain condition for iteration, like Equation (19),
As is a key parameter for filtering performance, it is difficult to select an appropriate value. Generally, when the Equation (19) is satisfied, should be maximized to achieve the highest robustness. However, the UAV navigation system is time-varying, the Equation (19) may not satisfy with a large . Thus, when in high-dynamic environment, the value of is set to be relatively smaller. is usually set at the range of in navigation. Here, represents error covariance matrix of ’s linear combination. Usually, is set to be equal to (unit matrix) in navigation.
Part 4. The Q matrix update equation is given as follows
where
and
respectively represent adjusting factors of Q-matrix and R-matrix. Essentially, they relate to the ratio between residual and historical information. The estimation’s adaptive and tracking skills rely more on current information. Therefore, we import forgetting factors
and
to reinforce the role of the residual. The recurrent equation is given as follows:
where
, and
. When the value of
is large enough, there are
and
. Generally, the value of
is between 0.9 and 0.999.