CN103344260A - Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (rao-black-wellised cubature kalman filter) - Google Patents

Abstract

The invention discloses an initial alignment method of a large azimuth misalignment angle of a strapdown inertial navigation system based on an RBCKF (rao-black-wellised cubature kalman filter), relates to the initial alignment method of the large azimuth misalignment angle of the strapdown inertial navigation system, and aims at solving the problems that the accuracy of the filtering method is low and easily diffused when the nonlinearity of the system is strong, and even EKF (exend kalman filter) filtering cannot be applied when the system is not continuous. The method comprises the following steps of: 1, building an error model for initial alignment of the large azimuth misalignment angle; 2, selecting an initial filtering value; 3, calculating a Cubature point set; 4, updating the time of a state variable and a measurement stable; and 5, updating a measurement equation. The initial alignment method is applied to the field of initial alignment of strapdown inertial navigation under the large azimuth misalignment angle.

Description

Initial alignment method for large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF

Technical Field

The invention relates to an initial alignment method for a large azimuth misalignment angle of a strapdown inertial navigation system.

Background

The purpose of filtering (i.e., state estimation) is to estimate a process of state vectors from noisy measurements according to certain methods and criteria. Through the development of recent decades from the classic kalman filtering from the beginning to the nonlinear filtering in various forms, the filtering technology has become a hot point for various industrial researches. Stability, accuracy, and convergence speed are issues of common concern for filtering.

The inertial navigation system (SINS) is mainly to measure the angular motion and linear motion of a carrier relative to an inertial space by using an inertia sensitive device (including a gyroscope and an accelerometer), and to further realize the navigation task expected by the carrier by using a computer to calculate navigation parameters such as the current speed information, position information, attitude information and the like of the carrier according to the known initial conditions.

For a strapdown inertial navigation system, all inertial measurement elements are directly fixed on a carrier, output data of the inertial measurement elements are angular velocity and acceleration of the carrier relative to an inertial space, and then the carrier is converted into a navigation coordinate system by a computer to perform navigation calculation, and the process is equivalent to that a platform is virtualized in the computer to be used as a reference for navigation calculation. The virtual platform functions similarly to a stable platform in a platform inertial navigation system, and is called as a mathematical platform. For the SINS itself, the accuracy and speed of the initial alignment play an important role in the SINS navigation process. Since the model of initial alignment of SINS has the characteristic of nonlinearity, a reasonable nonlinear filtering method is adopted according to specific requirements.

The earliest and most widely used nonlinear filtering method in engineering belongs to Extended Kalman Filtering (EKF). The core idea of the method is to linearize a nonlinear model at a fixed point. However, when the nonlinearity of the system is strong, the filtering method has low precision and is easy to diverge. Even when the system is discontinuous, EKF filtering cannot be applied. Based on the idea that "approximating a probability distribution is much easier than approximating a nonlinear function", Julier and Uhlmann propose Unscented Kalman Filtering (UKF). It approximates the mean and variance of the obeyed gaussian state distribution variables by deterministic Sigma points. Cubature Kalman Filter (CKF) was first proposed by Ienkaran Arasaratnam and Simon Haykin. The core idea of the method is Cufoundation transformation, namely, 2n Cufoundation points with equal weight values are selected to calculate the mean value and the variance of the variable subjected to nonlinear transformation and subjected to Gaussian distribution. The CKF filtering is derived according to the Spherical-radial Cubasic rule, and has better stability and higher precision compared with the UKF. From a filtering approach, CKF is a special case of the UKF. However, the number of sampling points of CKF is small, the calculation amount is small, and the filtering precision is better than that of UKF. Applying the successive Cubature transforms to the gaussian filter results in CKF filtering, which was originally proposed for the case where both the state equation and the metrology equation are non-linear.

Disclosure of Invention

The invention provides an initial alignment method of a large azimuth misalignment angle of a strapdown inertial navigation system based on RBCKF, and aims to solve the problems that when the nonlinearity of the system is strong, the accuracy of a filtering method is low, the filtering method is easy to disperse, and even when the system is discontinuous, EKF filtering cannot be applied.

The initial alignment method of the large azimuth misalignment angle of the strapdown inertial navigation system based on the RBCKF is realized by the following steps:

firstly, establishing an error model of large azimuth misalignment angle initial alignment, namely a state equation and a measurement equation, according to the error characteristic of a strapdown inertial navigation system;

step two, selecting a filtering initial value:

order to

And

wherein x is₀Is an initial value of a state variable, P₀An initial error covariance matrix for the state variables;

step three, according to the mean value of the state variables of the current moment

Covariance matrix P with state error_k|k-1To calculate the Cubasic point set: ${\mathrm{\χ}}_i=[{\hat{x}}_{k-1|k-1},{\hat{x}}_{k-1|k-1}+\sqrt{n*P_{k-1|k-1}},{\hat{x}}_{k-1|k-1}-\sqrt{n*P_{k-1|k-1}}];$ wherein the mean value of the state variablesCovariance matrix P with state error_k|k-1Calculating according to a traditional CKF filtering method;

wherein,

for one-step prediction of the mean value of the state variable at time k-1, P_k|k-1Predicting the state error covariance matrix at the k moment for the k-1 moment in one step;and P_k|k-1The calculation is obtained by weighting and accumulating Cubasic points at the moment of k-1 after nonlinear propagation of a state equation, the weight of each point is 1/2n, and n is the dimension of a system state variable;

step four, under the condition that the state equation is nonlinear and the measurement equation is linear, time updating of the state variable and the measurement variable is carried out;

$\left\{\begin{array}{c}{\mathrm{\χ}}_{i,k|k-1}^{*}=f\left({\mathrm{\χ}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m*{\mathrm{\χ}}_{i,k|k-1}^{*}\\ {\hat{z}}_{k|k-1}=H_k*{\hat{x}}_{k|k-1}\end{array}\right.$

step five, updating the error covariance matrix

$P_{{\hat{x}}_k{\hat{z}}_k}=P_{k|k-1}{H}_k^T$

$P_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+Q_k$

$P_{{\hat{z}}_k{\hat{z}}_k}=H_k{P}_{k|k-1}{H}_k^T+R_k$

And c, letting k = k +1, continuing to execute the step three, namely completing the initial alignment method of the large azimuth misalignment angle of the strapdown inertial navigation system based on the RBCKF.

The invention has the following effects:

aiming at a specific error model of initial alignment of a SINS large misalignment angle, a filtering method, namely, a Rao-Black-welled Cubalture Kalman Filter (RBCKF) is provided. When both the state equation and the measurement equation have nonlinearity, conventional CKF is a very excellent nonlinear filtering method. However, the equation of state of initial alignment of the SINS with a large misalignment angle has nonlinearity, and the measurement equation has the characteristic of linearity, so that the CKF can be improved, namely cubage transformation is adopted for the equation of state, and linear updating is adopted for the measurement equation. Since the transformed precision is the same for linear models, regardless of whether Cubature transform or linear transform is performed. Therefore, the RBCKF can reduce the time of SINS initial alignment without losing the filtering precision.

The basis of CKF filtering is the Cubature transform, which can be seen as a non-linear mapping of linear spaces of different dimensions. The method adopts some fixed points to approximate the mean value and the variance of the transformed variables. When the SINS is initially aligned at a large azimuth misalignment angle, the state equation has the characteristic of nonlinearity, and the measurement equation has the characteristic of linearity. Therefore, certain improvement can be made to CKF, so that the alignment time is shortened under the condition that the filtering accuracy is not changed.

Compared with the traditional CKF, the invention does not lose the filtering precision;

compared with CKF, the technology of the invention can shorten the initial alignment time of the SINS under a large azimuth misalignment angle;

the technology of the invention can be applied to any filter model with nonlinear state equation and linear measurement equation.

Drawings

FIG. 1 is a flow chart of the present invention;

FIG. 2 is a schematic diagram of RBCKF in the first embodiment;

FIG. 3 is a schematic diagram of initial alignment of SINS in a first embodiment;

FIG. 4 is a roll misalignment angle error curve in a simulation experiment; wherein-represents a CKF estimate, -represents a RBCKF estimate;

FIG. 5 is a pitch misalignment angle error plot in a simulation experiment; wherein-represents a CKF estimate, -represents a RBCKF estimate;

FIG. 6 is a course misalignment angle error curve in a simulation experiment; wherein-represents a CKF estimate, -represents a RBCKF estimate;

FIG. 7 is a steady state error curve for a heading misalignment angle in a simulation test; wherein-represents a CKF estimate, -represents a RBCKF estimate.

Detailed Description

The first embodiment is as follows: the initial alignment method for the large azimuth misalignment angle of the strapdown inertial navigation system based on the RBCKF is realized by the following steps:

firstly, establishing an error model of large azimuth misalignment angle initial alignment, namely a state equation and a measurement equation, according to the error characteristic of a strapdown inertial navigation system;

step two, selecting a filtering initial value:

order toAnd

wherein x is₀Is an initial value of a state variable, P₀An initial error covariance matrix for the state variables;

step three, according to the mean value of the state variables of the current moment

Covariance matrix P with state error_k|k-1To calculate the Cubasic point set: ${\mathrm{\χ}}_i=[{\hat{x}}_{k-1|k-1},{\hat{x}}_{k-1|k-1}+\sqrt{n*P_{k-1|k-1}},{\hat{x}}_{k-1|k-1}\sqrt{n*P_{k-1|k-1}}];$ wherein the mean value of the state variables

Covariance matrix P with state error_k|k-1Calculating according to a traditional CKF filtering method;

wherein,

for one-step prediction of the mean value of the state variable at time k-1, P_k|k-1Predicting the state error covariance matrix at the k moment for the k-1 moment in one step;

and P_k|k-1The calculation is obtained by weighting and accumulating Cubasic points at the moment of k-1 after nonlinear propagation of a state equation, the weight of each point is 1/2n, and n is the dimension of a system state variable;

step four, under the condition that the state equation is nonlinear and the measurement equation is linear, time updating of the state variable and the measurement variable is carried out;

$\left\{\begin{array}{c}{\mathrm{\χ}}_{i,k|k-1}^{*}=f\left({\mathrm{\χ}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m*{\mathrm{\χ}}_{i,k|k-1}^{*}\\ {\hat{z}}_{k|k-1}=H_k*{\hat{x}}_{k|k-1}\end{array}\right.$

step five, updating the error covariance matrix

$P_{{\hat{x}}_k{\hat{z}}_k}=P_{k|k-1}{H}_k^T$

$P_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+Q_k$

$P_{{\hat{z}}_k{\hat{z}}_k}=H_k{P}_{k|k-1}{H}_k^T+R_k$

And c, letting k = k +1, continuing to execute the step three, namely completing the initial alignment method of the large azimuth misalignment angle of the strapdown inertial navigation system based on the RBCKF.

In the second step, since the state space expression of the initial alignment of the SINS large azimuth misalignment angle is completely controllable and observable, and the system can be considered as consistent and stable, the selection of the initial filtering condition does not affect the accuracy of filtering convergence generally;

in the fifth step, as for the linear equation, the precision after transformation is the same no matter Cufoundation transformation or linear transformation. Therefore, the filtering accuracy of RBCKF and CKF is the same. However, from the view of the calculation amount, when the measurement mean, the variance and the cross-correlation covariance are calculated, the RBCKF does not adopt 2n Cubature points again to perform operations such as nonlinear transformation and square root decomposition, thereby saving the running time of the algorithm. At the same time, from $\left\{\begin{array}{c}{\mathrm{\χ}}_{i,k|k-1}^{*}=f\left({\mathrm{\χ}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m*{\mathrm{\χ}}_{i,k|k-1}^{*}\\ {\hat{z}}_{k|k-1}=H_k*{\hat{x}}_{k|k-1}\end{array}\right.$ The third equation of,

Formula (II) and

in the formula, RBCKF is performed at nm²+2n²m + mn multiplications and m²+nm²+2n²m + mn additions. In contrast, CKF not only needs to regenerate Cubasic point for non-linear transformation of measurement, but also needs to perform 2mn +4nm²+4n²m multiplication sums²+2nm²+2n²m+8nm+2n²And (4) secondary addition. It can be seen that as m and n increase, the calculation amount of RBCKF is much smaller than that of CKF.

Therefore, when the RBCKF filtering algorithm is applied to the initial alignment of the SINS large azimuth misalignment angle, the alignment time can be greatly shortened while the filtering precision is not lost.

The effect of the embodiment is as follows:

aiming at a specific error model of initial alignment of a SINS large misalignment angle, a filtering method, namely, a Rao-Black-welled Cubalture Kalman Filter (RBCKF) is provided. When both the state equation and the measurement equation have nonlinearity, conventional CKF is a very excellent nonlinear filtering method. However, the equation of state of initial alignment of the SINS with a large misalignment angle has nonlinearity, and the measurement equation has the characteristic of linearity, so that the CKF can be improved, namely cubage transformation is adopted for the equation of state, and linear updating is adopted for the measurement equation. Since the transformed precision is the same for linear models, regardless of whether Cubature transform or linear transform is performed. Therefore, the RBCKF can reduce the time of SINS initial alignment without losing the filtering precision.

The basis of CKF filtering is the Cubature transform, which can be seen as a non-linear mapping of linear spaces of different dimensions. The method adopts some fixed points to approximate the mean value and the variance of the transformed variables. When the SINS is initially aligned at a large azimuth misalignment angle, the state equation has the characteristic of nonlinearity, and the measurement equation has the characteristic of linearity. Therefore, certain improvement can be made to CKF, so that the alignment time is shortened under the condition that the filtering accuracy is not changed.

Compared with the traditional CKF, the implementation mode does not lose the filtering precision;

compared with CKF, the technology of the embodiment can shorten the initial alignment time of the SINS under a large azimuth misalignment angle;

the technique of this embodiment can be applied to any filter model in which the state equation is nonlinear and the measurement equation is linear.

The second embodiment is as follows: the first difference between the present embodiment and the specific embodiment is: the step one of establishing an error model of the large azimuth misalignment angle initial alignment specifically comprises the following steps:

selecting east and north speed errors in initial alignment of large azimuth misalignment angle of general static base

And misalignment angles phi of three orientations_x、φ_yAnd phi_zIs a state variable, i.e.

Therefore, the equation of state for initial alignment of its static base is as follows:

wherein,

the local latitude is; omega_ieThe rotational angular velocity of the earth;

and

gyro drift for three axes;andacceleration zero offset for three axial directions; f. of_x、f_yAnd f_zProjecting the proportional output value of the accelerometer on a computational geographical system; c_ijIs an element in the carrier to geographic direction matrix; c'_ijIs an element of the carrier to computational geography system matrix;

selecting east and north speed errors

For external observation, the measurement equation of initial alignment of the strapdown inertial navigation system is as follows: z = HX + V

Wherein H is H = [ I = [ ]_2×2 0_2×3](ii) a V is the measurement noise. Other steps and parameters are the same as those in the first embodiment.

The third concrete implementation mode: the present embodiment differs from the first or second embodiment in that: the Cubasic point set calculation in the third step is specifically as follows:

the error covariance matrix is subjected to square root decomposition, and then a Cubasic point set is selected, namely ${\mathrm{\χ}}_i=[{\hat{x}}_{k-1|k-1},{\hat{x}}_{k-1|k-1}+\sqrt{n*P_{k-1|k-1}},{\hat{x}}_{k-1|k-1}-\sqrt{n*P_{k-1|k-1}}].$ Other steps and parameters are the same as those in the first or second embodiment.

The fourth concrete implementation mode: the difference between this embodiment mode and one of the first to third embodiment modes is: the time updating of the state variables and the measured variables in the fourth step is specifically as follows: in the initial alignment of a large azimuth misalignment angle in a strapdown inertial navigation system, a state equation is nonlinear and a measurement equation is linear, and a specific mathematical expression is as follows:

$\left\{\begin{array}{c}\stackrel{.}{X}=f\left(X\right)+\mathrm{GW}\\ Z=\mathrm{HX}+V\end{array}\right.$

for the time updating of the state variable, the Cubasic point set in the third step is subjected to the nonlinear transformation of the state equation, and then the one-step prediction mean value of the state variable is calculated

The measured time is updated and not converted by Cubaure but directly converted by linear transformation to calculate the one-step predicted mean of the measured quantity

The specific process is calculated as follows:

$\left\{\begin{array}{c}{\mathrm{\χ}}_{i,k|k-1}^{*}=f\left({\mathrm{\χ}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m*{\mathrm{\χ}}_{i,k|k-1}^{*}\\ {\hat{z}}_{k|k-1}=H_k*{\hat{x}}_{k|k-1}\end{array}\right..$ other steps and parameters are the same as those in one of the first to third embodiments.

The fifth concrete implementation mode: the difference between this embodiment and one of the first to fourth embodiments is: step five, updating the error covariance matrix specifically as follows:

in the course of the filtered measurement update, the error covariance matrix P of the state variables needs to be calculated_k|k-1Error covariance matrix of measured variablesAnd cross-correlation covariance matrixThen, the variables are applied to a Gaussian filter to form an RBCKF filtering algorithm;

step five, first: in RBCKF, the error covariance matrix P of the state variables_k|k-1The calculation of (2) is the same as that of the traditional CKF calculation method, the calculation is still required to be carried out through the Cubasic point set, and the influence of state noise is included, specifically

Formula of calculation such as $P_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+Q_k$ Formula (I);

error covariance matrix of measured variables

And cross-correlation covariance matrix

Is different from the conventional CKF in that,

is directly updated by a linear equation,the fusion of the nonlinear equation and the linear equation needs to be performed, and the specific derivation is as follows:

$P_{{\hat{z}}_k{\hat{z}}_k}=E\left[{(H_k{\hat{x}}_{k|k-1}-H_k{x}_k-v_k)(H_k{\hat{x}}_{k|k-1}-H_k{x}_k-v_k)}^T\right]$

$=H_{k}E\left[({\hat{x}}_{k|k-1}-x_k){({\hat{x}}_{k|k-1}-x_k)}^T\right]H_k^T+E\left[v_k{v}_k^T\right]$

$=H_k{P}_{k|k-1}{H}_k^T+R_k$

$P_{{\hat{x}}_k{\hat{z}}_k}=E\left[({\hat{x}}_{k|k-1}-x_k){(H_k{\hat{x}}_{k|k-1}-H_k{x}_k-v_k)}^T\right]$

$=E\left[({\hat{x}}_{k|k-1}-x_k){({\hat{x}}_{k|k-1}-x_k)}^T\right]H_k^T+E\left[({\hat{x}}_{k|k-1}-x_k)v_k^T\right]$

$=E\left[({\hat{x}}_{k|k-1}-x_k){({\hat{x}}_{k|k-1}-x_k)}^T\right]H_k^T+E\left[({\hat{x}}_{k|k-1}-x_k)\right]*E\left[v_k^T\right]$

$=E[({\hat{x}}_{k|k-1}-x_k){({\hat{x}}_{k|k-1}-x_k)}^T{]H}_k^T=P_{k|k-1}{H}_k^T$

step five two: will be provided withAnd

the RBCKF is generated by reapplying under a Gaussian filter, which is generally of the form:

$\left\{\begin{array}{c}{x}_{k|k-1}={\hat{x}}_{k|k-1}+W_k(z_k-{\hat{z}}_k)\\ P_{k|k}=P_{k|k-1}-W_k{P}_{{\hat{z}}_k{\hat{z}}_k}{W_k}^T\\ W_k=P_{{\hat{x}}_k{\hat{z}}_k}{P_{{\hat{z}}_k{\hat{z}}_k}}^{-1}\end{array}\right.$

step five and step three: compared with CKF, RBCKF can greatly shorten the convergence time of filtering without losing filtering precision, and the specific comparison is as follows:

assuming the measurement equation to be: z_k=H_kX_k+V_k(wherein, H_kM × n matrix) from the whole filtering algorithm, the RBCKF and CKF are just calculating the measured mean value

Measurement variance

And cross-correlation covariance matrixThere is a difference, and in the conventional CKF, the calculation formulas of the three terms are as follows:

$\left\{\begin{array}{c}{\mathrm{\χ}}_{k|k-1}=\left[\begin{array}{ccc}{\hat{x}}_{k|k-1}& {\hat{x}}_{k|k-1}+\sqrt{n*P_{k|k-1}}& {\hat{x}}_{k|k-1}-\sqrt{n-P_{k|k-1}}\end{array}\right]\\ {\mathrm{\η}}_{i,k|k-1}=h\left({\mathrm{\χ}}_{i,k|k-1}\right)\\ {\hat{z}}_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(m\right)}{\mathrm{\η}}_{i,k|k-1}\end{array}\right.$

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(c\right)}({\mathrm{\η}}_{i,k|k-1}-{\hat{z}}_{k\{k-1}){({\mathrm{\η}}_{i,k|k-1}-{\hat{z}}_{k\{k-1})}^T+R_k$

$P_{{\hat{x}}_k{\hat{z}}_k}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(c\right)}({\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k\{k-1}){({\mathrm{\η}}_{i,k|k-1}-{\hat{z}}_{k\{k-1})}^T.$

other steps and parameters are the same as in one of the first to fourth embodiments.

The following simulation tests were performed to verify:

before initial alignment of SINS, selecting an initial value x (0) =0 of a state variable; the initial east, north and sky misalignment angles are set to: a first group: phi is a_E=φ_N=0.5⁰,φ_U=10⁰(ii) a Second group: phi is a_E=φ_N=0.5⁰,φ_U=15⁰(ii) a Third group: phi is a_E=φ_N=0.5⁰,φ_U=20⁰(ii) a According to the calculation of the medium-precision gyro, the constant drift of the gyro is assumed to be 0.1⁰H, random drift of 0.05⁰H; accelerometer zero offset of 1.0 x 10^-4m/s²Random deviation of 0.5X 10^-4g；

The filter parameters are set as follows:

the initial error covariance matrix for the state variable x is P₀=diag{(0.1m/s)² (0.1m/s)² (0.5⁰)² (0.5⁰)² (10⁰)²}; an accelerometer is attached toAnd the machine deviation and the random drift of the gyroscope are recorded as process noise, and the noise error covariance matrix of the state equation is Q = diag { (50ug)²(50ug)²(0.05⁰/h)²(0.05⁰/h)²(0.05⁰/h)²}; meanwhile, the covariance matrix of the noise error of the measurement equation is recorded as R = diag { (0.1m/s)²(0.1m/s)²}. The filter algorithm running times for CKF and RBCKF at different azimuthal misalignment angles are shown in table 1:

TABLE 1 calculation time of CKF and RBCKF at different initial azimuth misalignment angles

The run times of the two filtering algorithms for different large azimuth misalignment angles are given in table 1. Because the measurement equation is linear, the RBCKF adopts a linear updating mode during time updating, and compared with the mode that the traditional CKF filtering adopts 2n points, the calculation amount of the RBCKF is small. Therefore, the running time of the RBCKF is always smaller than that of the conventional CKF filtering under the same azimuth misalignment angle. That is, the RBCKF can improve the alignment speed of inertial navigation initial alignment.

FIGS. 4 to 7 show the equation at phi_x=φ_y=0.5⁰,φ_z=10⁰Lower roll, pitch and heading estimated misalignment angle error curves.

Since the initial misalignment angle is only large for azimuth misalignment angles and small for horizontal misalignment angles. Thus, FIG. 7 provides a steady state error plot of the azimuthal misalignment angle error.

It can be seen from FIG. 7 that the steady state error values for CKF and RBCKF for the heading misalignment angle are the same, both at-3.588'. Therefore, the filtering accuracy of the two filtering methods is the same.

In summary, compared with the conventional CKF filtering algorithm, the filtering precision of the RBCKF filtering algorithm is the same, but from the aspect of algorithm running time, the running time of the RBCKF is always smaller than that of the conventional CKF under the same large azimuth misalignment angle. Therefore, the RBCKF can help to shorten the alignment time of the strapdown inertial navigation system without losing the filtering precision.

Claims (5)

1. The method for initially aligning the large azimuth misalignment angle of the strapdown inertial navigation system based on the RBCKF is characterized in that the method for initially aligning the large azimuth misalignment angle of the strapdown inertial navigation system based on the RBCKF is realized according to the following steps:

firstly, establishing an error model of large azimuth misalignment angle initial alignment, namely a state equation and a measurement equation, according to the error characteristic of a strapdown inertial navigation system;

step two, selecting a filtering initial value:

order to ${\hat{x}}_0=E\left[x_0\right]$ And $P_0=\left[{(x_0-{\hat{x}}_0)(x_0-{\hat{x}}_0)}^T\right];$ wherein x is₀Is an initial value of a state variable, P₀An initial error covariance matrix for the state variables;

step three, according to the mean value of the state variables of the current moment

Covariance matrix P with state error_k|k-1To calculate the Cubasic point set: ${\mathrm{\χ}}_i=[{\hat{x}}_{k-1|k-1},{\hat{x}}_{k-1|k-1}+\sqrt{n*P_{k-1|k-1}},{\hat{x}}_{k-1|k-1}-\sqrt{n*P_{k-1|k-1}}];$ wherein the mean value of the state variablesCovariance matrix P with state error_k|k-1Calculating according to a traditional CKF filtering method;

wherein,

for one-step prediction of the mean value of the state variable at time k-1, P_k|k-1Predicting the state error covariance matrix at the k moment for the k-1 moment in one step;and P_k|k-1The calculation is obtained by weighting and accumulating Cubasic points at the moment of k-1 after nonlinear propagation of a state equation, the weight of each point is 1/2n, and n is the dimension of a system state variable;

step four, under the condition that the state equation is nonlinear and the measurement equation is linear, time updating of the state variable and the measurement variable is carried out;

$\left\{\begin{array}{c}{\mathrm{\χ}}_{{i,}^{k|k-1}}^{*}=f\left({\mathrm{\χ}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m*{\mathrm{\χ}}_{{i,}^{k|k-1}}^{*}\\ {\hat{z}}_{k|k-1}=H_k*{\hat{x}}_{k|k-1}\end{array}\right.$

step five, updating the error covariance matrix

$P_{{\hat{x}}_k{\hat{z}}_k}=P_{k-|k-1}{H}_k^T$

$P_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c{({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+Q_k$

$P_{{\hat{z}}_k{\hat{z}}_k}=H_k{P}_{k|k-1}{H}_k^T+R_k$

And c, taking k as k +1, and continuing to execute the step three, namely completing the initial alignment method of the large azimuth misalignment angle of the strapdown inertial navigation system based on the RBCKF.

2. The method for initial alignment of a large azimuth misalignment angle of a strapdown inertial navigation system based on RBCKF as claimed in claim 1, wherein the error model for establishing the initial alignment of the large azimuth misalignment angle in the step one is specifically:

selecting east-north speed error delta v in initial alignment of large azimuth misalignment angle of general static base_x、δv_yAnd misalignment angles phi of three orientations_x、φ_yAnd phi_zIs a state variable, i.e. X ═ δ v_xδv_yφ_xφ_yφ_z]^TTherefore, the equation of state for initial alignment of its static base is as follows:

wherein,the local latitude is; omega_ieThe rotational angular velocity of the earth;

and

gyro drift for three axes;

and

acceleration zero offset for three axial directions; f. of_x、f_yAnd f_zProjecting the proportional output value of the accelerometer on a computational geographical system; c_ijIs an element in the carrier to geographic direction matrix; c_ijIs an element of the carrier to computational geography system matrix;

selecting east and north velocity errors delta v_x，δv_yFor external observation, the measurement equation of initial alignment of the strapdown inertial navigation system is as follows:

Z＝HX+V

wherein H is H ═ I_2×20_2×3](ii) a V is the measurement noise.

3. The method for initial alignment of a large azimuth misalignment angle of a strapdown inertial navigation system based on RBCKF as claimed in claim 1, wherein the calculating the Cubasic point set in the third step is specifically:

the error covariance matrix is subjected to square root decomposition, and then a Cubasic point set is selected, namely ${\mathrm{\χ}}_i=[{\hat{x}}_{k-1|k-1},{\hat{x}}_{k-1|k-1}+\sqrt{n*P_{k-1|k-1}},{\hat{x}}_{k-1|k-1}-\sqrt{n*P_{k-1|k-1}}].$

4. The method for initial alignment of a large azimuth misalignment angle of a strapdown inertial navigation system based on RBCKF as claimed in claim 1, wherein the time update of the state variables and the measurement variables in the fourth step is specifically: in the initial alignment of a large azimuth misalignment angle in a strapdown inertial navigation system, a state equation is nonlinear and a measurement equation is linear, and a specific mathematical expression is as follows:

$\left\{\begin{array}{c}\stackrel{\•}{X}=f\left(X\right)+\mathrm{GW}\\ Z=\mathrm{HX}+V\end{array}\right.$

for the time updating of the state variable, the Cubasic point set in the third step is subjected to the nonlinear transformation of the state equation, and then the one-step prediction mean value of the state variable is calculatedThe measured time is updated and the one-step prediction mean value of the linear transformation calculated quantity measurement is directly carried out without Cubasic transformation

The specific process is calculated as follows:

$\left\{\begin{array}{c}{\mathrm{\χ}}_{{i,}^{k|k-1}}^{*}=f\left({\mathrm{\χ}}_{i,k-1}\right)\\ {\hat{x}}_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^m*{\mathrm{\χ}}_{{i,}^{k|k-1}}^{*}\\ {\hat{z}}_{k|k-1}=H_k*{\hat{x}}_{k|k-1}\end{array}.\right.$

5. the method for initial alignment of a large azimuth misalignment angle of a strapdown inertial navigation system based on RBCKF as claimed in claim 1, wherein the step five of updating the error covariance matrix specifically comprises:

in the course of the filtered measurement update, the error covariance matrix P of the state variables needs to be calculated_k|k-1Error in measuring a variableDifference covariance matrix

And cross-correlation covariance matrix

Then, the variables are applied to a Gaussian filter to form an RBCKF filtering algorithm;

step five, first: in RBCKF, the error covariance matrix P of the state variables_k|k-1The calculation of (2) is the same as that of the conventional CKF calculation method, and still needs to be calculated through the Cubasic point set and to include the influence of state noise, wherein the specific calculation formula is as follows $P_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^c{({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+Q_k$ Formula (I);

error covariance matrix of measured variablesAnd cross-correlation covariance matrixIs different from the conventional CKF in that,is directly updated by a linear equation,

the fusion of the nonlinear equation and the linear equation needs to be performed, and the specific derivation is as follows:

$P_{{\hat{z}}_k{\hat{z}}_k}=E\left[(H_k{\hat{x}}_{k|k-1}-H_k{x}_k-v_k){(H_k{\hat{x}}_{k|k-1}-H_k{x}_k-v_k)}^T\right]$

$=H_{k}E\left[({\hat{x}}_{k|k-1}-x_k){({\hat{x}}_{k|k-1}-x_k)}^T\right]H_k^T+E\left[v_k{v}_k^T\right]$

$=H_k{P}_{k|k-1}{H}_k^T+R_k$

$P_{{\hat{x}}_k{\hat{z}}_k}=E\left[({\hat{x}}_{k|k-1}-x_k){(H_k{\hat{x}}_{k|k-1}-H_k{x}_k-v_k)}^T\right]$

$=E[({\hat{x}}_{k|k-1}-x_k){({\hat{x}}_{k|k-1}-x_k)}^T{H}_k^T({\hat{x}}_{k|k-1}-x_k)v_k^T]$

$=E\left[({\hat{x}}_{k|k-1}-x_k){({\hat{x}}_{k|k-1}-x_k)}^T\right]H_k^T+E\left[({\hat{x}}_{k|k-1}-x_k)\right]*E\left[v_k^T\right]$

$=E\left[\left({\hat{x}}_{k|k-1}\right){({\hat{x}}_{k|k-1}-x_k)}^T\right]H_k^T=P_{k|k-1}{H}_k^T$

step five two: will be provided with

P_k|k-1、

And

the RBCKF is generated by reapplying under a Gaussian filter, which is generally of the form:

$\left\{\begin{array}{c}{x}_{k|k}={\hat{x}}_{k|k-1}+W_k(z_k-{\hat{z}}_k)\\ P_{k|k}=P_{k|k-1}-W_k{P}_{{\hat{z}}_k{\hat{z}}_k}{W_k}^T\\ W_k=P_{{\hat{x}}_k{\hat{z}}_k}{P}_{{\stackrel{6}{z}}_k{\hat{z}}_k}\end{array}\right.$

step five and step three: compared with CKF, RBCKF can greatly shorten the convergence time of filtering without losing filtering precision, and the specific comparison is as follows:

assuming the measurement equation to be: z_k＝H_kX_k+V_k(wherein, H_kM × n matrix) from the whole filtering algorithm, the RBCKF and CKF are just calculating the measured mean value

Measurement variance

And cross-correlation covariance matrix

There is a difference, and in the conventional CKF, the calculation formulas of the three terms are as follows:

$\left\{\begin{array}{c}{\mathrm{\χ}}_{k|k-1}=[{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}+\sqrt{n*P_{k|k-1}}{\hat{x}}_{k|k-1}-\sqrt{n*P_{k|k-1}}]\\ {\mathrm{\η}}_{i,k|k-1}=h\left({\mathrm{\χ}}_{i,k|k-1}\right)\\ {\hat{z}}_{k|k-1}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(m\right)}{\mathrm{\η}}_{i,k|k-1}\end{array}\right.$

$P_{{\hat{z}}_k{\hat{z}}_k}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(c\right)}({\mathrm{\η}}_{i,k|k-1}-{\hat{z}}_{k\{k-1}){({\mathrm{\η}}_{i,k|k-1}-{\hat{z}}_{k\{k-1})}^T+R_k$

$P_{{\hat{x}}_k{\hat{z}}_k}=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(c\right)}({\mathrm{\χ}}_{i,k|k-1}-{\hat{x}}_{k\{k-1}){({\mathrm{\η}}_{i,k|k-1}-{\hat{z}}_{k\{k-1})}^T.$

Images