阅读说明：本技术 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法 (unmanned ship integrated navigation method based on adaptive federal Kalman filtering ) 是由 王宁 杨毅 李贺 付水 陈帅 于 2019-09-17 设计创作，主要内容包括：本发明公开了一种基于自适应联邦卡尔曼滤波的无人船组合导航方法,利用无人船组合导航系统进行导航,所述无人船组合导航系统包括SINS、GPS、Compass和嵌入式处理器。所述嵌入式处理器内采用自适应联邦卡尔曼滤波算法进行传感器信息融合,输出无人船的位置、速度和姿态信息。本发明应用了无人船SINS/GPS/Compass组合导航的误差模型和观测模型,减小各个子系统之间的故障干扰,提高无人船导航系统的可靠性与稳定性。本发明在联邦卡尔曼滤波的基础上,设计联邦卡尔曼滤波的子滤波器的信息分配因子,在保障系统的容错能力的前提下,能够有效抑制系统的异常扰动,减小分配原则对滤波精度的影响。(the invention discloses an unmanned ship integrated navigation method based on self-adaptive Federal Kalman filtering, which utilizes an unmanned ship integrated navigation system to carry out navigation, wherein the unmanned ship integrated navigation system comprises SINS, GPS, Compass and an embedded processor. And the embedded processor performs sensor information fusion by adopting a self-adaptive Federal Kalman filtering algorithm and outputs the position, speed and attitude information of the unmanned ship. The invention applies an error model and an observation model of unmanned ship SINS/GPS/Compass combined navigation, reduces fault interference among subsystems and improves the reliability and stability of the unmanned ship navigation system. According to the invention, on the basis of the Federal Kalman filtering, the information distribution factor of the sub-filter of the Federal Kalman filtering is designed, so that the abnormal disturbance of the system can be effectively inhibited on the premise of ensuring the fault-tolerant capability of the system, and the influence of the distribution principle on the filtering precision is reduced.)

1. an unmanned ship integrated navigation method based on adaptive federal Kalman filtering is characterized in that: navigating by using an unmanned ship integrated navigation system, wherein the unmanned ship integrated navigation system comprises a strapdown inertial navigation system sensor (SINS), a global positioning system sensor (GPS), a three-dimensional electronic Compass sensor (Compass) and an embedded processor; the embedded processor adopts a self-adaptive Federal Kalman filtering algorithm to perform sensor information fusion and outputs the position, speed and attitude information of the unmanned ship; the self-adaptive Federal Kalman filtering algorithm is realized based on a self-adaptive Federal Kalman filter; the self-adaptive Federal Kalman filter comprises a main filter, an SINS/GPS sub-filter, an SINS/Compass sub-filter and a self-adaptive information distribution factor calculation module; the main filter is respectively connected with the SINS/GPS sub-filter and the SINS/Compass sub-filter in a bidirectional way; the SINS/GPS sub-filter outputs a state estimation value and an estimation error covariance matrix of position and speed information, and the SINS/Compass sub-filter outputs a state estimation value and an estimation mean square error covariance matrix of attitude information; the adaptive information distribution factor calculation module calculates the adaptive information distribution factor through the estimated mean square error covariance matrix of the predicted state vectors of the two sub-filters, and sends the normalized adaptive information distribution factor to the main filter; performing a time updating process and an information fusion process in the main filter, performing information fusion on state estimation values and state estimation mean square error covariance matrixes of the two sub-filters and the state estimation value of the main filter to obtain a global optimal state estimation value, feeding the global optimal state estimation value back to the two sub-filters, and resetting the sub-filters;

the integrated navigation method comprises the following steps:

A. Establishing state equation of unmanned ship integrated navigation system

selecting a northeast geographic coordinate system as a navigation coordinate system, and considering error factors of a strapdown inertial navigation system, an accelerometer and a gyroscope by using a state equation, wherein the state equation of the integrated navigation system is as follows:

wherein the content of the first and second substances,The state estimation value of the integrated navigation system is shown, F is a state transition matrix of the integrated navigation system, X is a state variable of the integrated navigation system, G is a control matrix of the integrated navigation system, and W is a white noise matrix of the integrated navigation system;

the state variable X of the integrated navigation system is:

In the formula (I), the compound is shown in the specification,the attitude error angles in the east, north and sky directions of the unmanned ship are respectively; delta V_E、δV_N、δV_Uthe speed errors in the east, north and sky directions of the unmanned ship are respectively; delta lambda, delta L and delta h are errors of the latitude, longitude and altitude of the unmanned ship respectively; epsilon_bx、ε_by、ε_bzRespectively the ordinary drift of the gyroscope in east, north and sky directions under the carrier system; epsilon_rx、ε_ry、ε_rzthe slow-changing drifts of the gyroscope in east, north and sky directions under the carrier system are respectively; respectively are constant random errors of the accelerometer in east, north and sky directions under the carrier system;

the white noise matrix W of the integrated navigation system is as follows:

W＝[ω_gx,ω_gy,ω_gz,ω_rx,ω_ry,ω_rz,ω_ax,ω_ay,ω_az]^T

wherein, ω is_gx、ω_gy、ω_gzWhite noise in the east, north and sky directions of the gyroscope respectively; omega_rx、ω_ry、ω_rzwhite noise is driven by the first-order Markov process in the east, north and sky directions of the gyroscope respectively; omega_ax、ω_ay、ω_azdriving white noise for a first order Markov process in east, north, and sky directions of an accelerometer;

The error model of the gyroscope is:

ε＝ε_b+ε_r+ω_g

in the formula, epsilon_bIs a random constant; epsilon_rRandom noise, epsilon, for a first order Markov process_rSatisfies the following formula: a

T_gFor time, ω, of gyroscope_rDriving white noise for the first order markov process of the gyroscope;

The error model of the accelerometer is:

in the formula (I), the compound is shown in the specification,In order to be an error of the accelerometer,T_afor time, omega, of accelerometer correlation_awhite noise is driven for the first order markov process of the accelerometer,Is the velocity error of the accelerometer;

The state transition matrix of the integrated navigation system is as follows:

in the formula, F_Nthe system matrix of basic navigation parameters is 9 rows and 9 columns, and the specific steps are as follows:

F_N(2,7)＝-ω_ie sinL

F_N(4,2)＝-f_U

F_N(5,1)＝f_U

F_N(5,3)＝-f_E

F_N(6,1)＝-f_N

F_N(6,2)＝-f_E

F_N(6,7)＝-2V_Eω_ie sinL

F_N(9,6)＝1

the other elements are 0;

Wherein ω is_ieIs the angular rate of rotation of the earth, f_E、f_Uthe oblateness of the earth, R, in the east and the sky directions respectively_Eis the equatorial radius of the earth, R_MA principal radius of curvature in a meridian at any point on the surface of the earth; r_NThe main curvature radius of any point on the earth surface along the unitary-mortise circle; l is longitude;

F_M＝diag{0，0,0,-1/T_rx,-1/T_ry,-1/T_rz,-1/T_ax,-1/T_ay,-1/T_az}

In the formula:

θ、Psi is roll angle, pitch angle and course angle of the unmanned ship respectively;

The control matrix of the integrated navigation system is as follows:

B. Designing SINS/GPS sub-filter

The SINS/GPS sub-filter adopts a position and speed combined mode, and the measurement equation of the position and the speed of the SINS/GPS sub-filter is established as follows:

Z₁＝HX₁+V₁

Z₁as a measure of the SINS/GPS sub-filter, H₁Is a measurement matrix of SINS/GPS sub-filters, V₁measuring noise of the SINS/GPS sub-filter;

The speed information of the SINS is:

The speed information of the GPS is:

The position information of the SINS is as follows:

The position information of the GPS is:

In the formula, v_IE、v_IN、v_IURespectively velocity information of the SINS in east, north and sky directions; v. of_GE、v_GN、v_GUrespectively speed information of east, north and sky directions of the GPS; lambda [ alpha ]_I、L_I、h_IRespectively position information of the SINS in east, north and sky directions; lambda [ alpha ]_G、L_G、h_GPosition information of east, north and sky directions of the GPS respectively; v. of_N、v_E、v_Uthe speed truth values of the directions of the unmanned ship east, north and sky are respectively; lambda [ alpha ]_t、L_t、h_tPosition truth values of latitude, longitude and altitude of the unmanned ship are respectively; delta v_E、δv_N、δv_URespectively the velocity errors in the east, north and sky directions of the SINS; m_N、M_E、M_UThe speed errors in the east, north and sky directions of the GPS are respectively; δ λ, δ L, δ h are errors of latitude, longitude, altitude of SINS respectively, N_N、N_E、N_Uerrors of latitude, longitude and altitude of the GPS are respectively;

Taking the velocity difference value of the SINS and the GPS as the measurement value of the SINS/GPS sub-filter, and defining a velocity measurement equation as follows:

in the formula:

Z_Vas a measure of the SINS/GPS sub-filter velocity information, H_VIs a measurement matrix, V, of SINS/GPS sub-filter velocity information_VMeasurement noise, O, for SINS/GPS sub-filter velocity information_3×3zero matrix of 3 rows and 3 columns, O_3×12A zero matrix of 3 rows and 12 columns;

Taking the position difference value of the SINS and the GPS as the measurement value of the SINS/GPS sub-filter, and defining a position measurement equation as follows:

In the formula:

V_p＝[N_N,N_E,N_U]^T

R_M＝R_e(1-2f+3fsin²L)

R_N＝R_e(1+fsin²L)

In the formula: z_PAs a measure of the SINS/GPS sub-filter velocity information, H_PIs a measurement matrix, V, of SINS/GPS sub-filter velocity information_Pmeasurement noise, V, for SINS/GPS sub-filter velocity information_PProcessing as white noise; r_eis the equatorial radius of the earth; taking 6378137 m; f is the oblateness of the earth, and 1/298.257 is taken; o is_3×6A zero matrix with 3 rows and 6 columns; o is_3×9A zero matrix of 3 rows and 9 columns;

The combined mode of speed and position is adopted to establish a measurement equation of the SINS/GPS sub-filter as follows:

C. Designing SINS/Compass sub-filters

the measurement equation for establishing the attitude of the unmanned ship integrated navigation SINS/Compass sub-filter is as follows:

Z₂＝H₂X+V₂

In the formula, Z₂Measured values for the SINS/Compass sub-filters, H₂is a measurement matrix of SINS/Compass sub-filters, V₂The measurement noise of the SINS/Compass sub-filter;

the posture information of the SINS is as follows:

compass pose information is:

in the formula (I), the compound is shown in the specification,Respectively representing the posture information of the east, north and sky directions of the SINS;Posture information of east, north and sky of Compass respectively;posture truth values of the unmanned ship in east, north and sky directions are respectively;respectively representing attitude errors in east, north and sky directions of the SINS; delta alpha_E、δα_N、δα_UAttitude errors in east, north and sky directions of Compass respectively;

And taking the difference value of the attitude information output by the SINS and the Compass as the measured value of the SINS/Compass sub-filter, wherein the observation equation is as follows:

in the formula, H₂＝[I_3×3,0_3×15]，I_3×3Is an identity matrix of 3 rows and 3 columns, O_3×15Is a 3-row 15-column identity matrix, V₂＝[δα_c,δβ_c,δγ_c]^TIs the observed white noise with a mean of zero;

D. Process for implementing adaptive Federal Kalman Filter

The SINS/GPS sub-filter and the SINS/Compass sub-filter run in parallel, the SINS, GPS and Compass data are subjected to distributed processing, and information fusion is carried out in the main filter; the estimation error covariance arrays and the state estimation values of the two sub-filters are sent to the main filter, and information fusion is carried out on the estimation values of the main filter to obtain global optimal estimation; feeding back the global optimal estimation to the sub-filters according to an information distribution principle to complete primary filtering, and resetting the estimation values of the two sub-filters;

the information fusion process of the self-adaptive Federal Kalman filter is carried out in the main filter, and comprises four steps of information distribution, time updating, measurement updating and information fusion, and the method comprises the following specific steps:

d1, information distribution

And distributing the global state estimation, the error variance matrix and the noise information to two sub-filters and a main filter, wherein the distribution principle is as follows:

Wherein i-1 and 2 represent the SINS/GPS sub-filter and SINS/Compass sub-filter, respectively, and P_i,k-1is the estimated mean square error covariance matrix, P, at the time of sub-filter k-1_g,k-1Estimating mean square error covariance matrix, Q, for global optimum at time k-1_i,k-1The system noise variance matrix, Q, at the moment of sub-filter noise k-1_g,k-1The system noise variance matrix at the moment of main filter noise k-1,For the state estimate at time instant k-1 of the sub-filter,Estimating the state of the main filter at the moment k-1;

Information distribution factor beta_isatisfy the principle of information conservation, i.e.

No information is distributed in the main filter, and state estimation and time updating of covariance matrix are performed, i.e. P_m,k＝0；

D2, time update

Time updating is independently carried out between the two sub-filters, and state prediction vectors and a one-step prediction mean square error covariance matrix of the two sub-filters are respectively obtained:

Wherein i-1 and 2 represent the SINS/GPS sub-filter and SINS/Compass sub-filter, respectively, and P_i,k|k-1the mean square error covariance matrix is predicted for one step of the sub-filters,for predicting the state of the sub-filters, phi_i,k|k-1Is a one-step transfer matrix, Γ, from sub-filter time k-1 to time k_i,k-1A noise driving array;

D3, measurement update

The two sub-filters receive the measurement information and independently perform a measurement updating process, respectively obtain state estimation values, filter gains and estimation mean square error covariance matrixes of the two sub-filters, and transmit the state estimation values, the filter gains and the estimation mean square error covariance matrixes to the main filter:

wherein i-1 and 2 represent SINS/GPS sub-filter and SINS/Compass sub-filter respectively,For state estimation at sub-filter time K, K_i,kIs the filter gain at time k of the sub-filter, P_i,kfor estimating mean square error covariance matrix，Z_i,kAs a measure of the sub-filter at time k, H_i,kMeasurement matrix of sub-filter k time, R_i,kMeasuring a noise variance matrix and an I unit matrix at the moment k of the sub-filter;

The measurement updating process is only carried out in the sub-filter, and the time updating process is only carried out in the main filter;

WhereinFor the state estimate at the moment of the main filter k-1,predicting a vector for the state of the main filter at the moment k-1;

d4, information fusion

And fusing the state estimation information of the two sub-filters and the state estimation of the main filter to obtain global state estimation information:

in the formula, P_g,kEstimating the mean square error covariance matrix, P, for a global optimum at time k_1,kIs an estimated mean square error covariance matrix, P, at time k of the SINS/GPS sub-filter_2,kis an estimated mean square error covariance matrix, P, at time k of the SINS/Compass sub-filter_m,kis the estimated mean squared error covariance matrix at the moment k of the main filter,estimating a global optimal state at the moment k;

D5 calculating adaptive information distribution factor

The optimal adaptive factor satisfies the following conditions:

the optimal adaptation factor is expressed as follows:

The deviation of the geometric state vector from the predicted state vector is:

Wherein the geometric state vectorcomprises the following steps:

In the formula (E)_i,kIs Z_i,kthe equivalent weight matrix of (2);

wherein the content of the first and second substances,To predict the state vector, P_i,k|k-1to predict the estimated covariance matrix of the state vector,A theoretical covariance matrix for the predicted state vector;

When the estimator is based on the predicted state vector at the current time, the estimated covariance matrix of the predicted state vector is as follows:

Optimal adaptive factor alpha_kis less than 1, expressed as:

in the formula:

the estimated covariance matrix of the optimal adaptation factor based on the prediction state vector is then expressed as:

the adaptation factor represented by the above formula is similar to the adaptation factor constructed from the difference between the predicted state of the model and the measured estimated state;

the adaptation factor is expressed as a prediction state vector in the form:

Wherein c is a constant,based on prediction of state vectorsthe statistics of the structure are expressed as:

The method for calculating the optimal adaptive factor based on the prediction state vector comprises the following steps of solving the adaptive information distribution factor obtained from the adaptive Federal Kalman filter as follows:

Wherein b is a constant and is 0.85-1.0;

In order to ensure that the self-adaptive information distribution factors meet the information conservation law, the self-adaptive information distribution factors are subjected to normalization processing; the adaptive information allocation factor is normalized to:

in the formula, beta_i'_,kand distributing factors for the adaptive information of the normalized sub-filters at the k moment.

Technical Field

The invention belongs to the field of sensor information fusion, and particularly relates to an unmanned ship integrated navigation method based on self-adaptive federal Kalman filtering.

Background

due to the flexibility and versatility of offshore applications, unmanned ships have wide applications in various aspects such as military production. The Federal Kalman filtering is applied to unmanned ships, unmanned planes and unmanned vehicles, the integrated navigation can detect the faults of navigation subsystems, and the navigation information of normal subsystems is combined, so that the fault tolerance and the self-adaptability of the system are improved. Methods such as Kalman filtering, particle filtering, federal filtering and the like are widely used for information fusion of the integrated navigation sensor. Aiming at the field of information fusion of multiple sensors, the Federal Kalman filtering effect is most obvious, but the filtering precision of the Federal Kalman filtering effect is influenced by an information distribution principle, and the filtering precision is low when abnormal disturbance occurs to a carrier.

Disclosure of Invention

In order to solve the problems in the prior art, the invention aims to design an unmanned ship integrated navigation method based on adaptive federal Kalman filtering, which can improve the filtering precision.

in order to achieve the purpose, the technical scheme of the invention is as follows:

an unmanned ship integrated navigation method based on self-adaptive Federal Kalman filtering utilizes an unmanned ship integrated navigation system to carry out navigation, wherein the unmanned ship integrated navigation system comprises a strapdown inertial navigation system sensor (SINS), a global positioning system sensor (GPS), a three-dimensional electronic Compass sensor (Compass) and an embedded processor. And the embedded processor performs sensor information fusion by adopting a self-adaptive Federal Kalman filtering algorithm and outputs the position, speed and attitude information of the unmanned ship. The adaptive federal Kalman filtering algorithm is realized based on an adaptive federal Kalman filter. The self-adaptive Federal Kalman filter comprises a main filter, an SINS/GPS sub-filter, an SINS/Compass sub-filter and a self-adaptive information distribution factor calculation module; and the main filter is respectively connected with the SINS/GPS sub-filter and the SINS/Compass sub-filter in a bidirectional way. The SINS/GPS sub-filter outputs a state estimation value and an estimation error covariance matrix of the position and speed information, and the SINS/Compass sub-filter outputs a state estimation value and an estimation mean square error covariance matrix of the attitude information. And the self-adaptive information distribution factor calculation module calculates the self-adaptive information distribution factor through the estimated mean square error covariance matrix of the predicted state vectors of the two sub-filters, and sends the normalized self-adaptive information distribution factor to the main filter. And performing a time updating process and an information fusion process in the main filter, performing information fusion on the state estimation values and the state estimation mean square error covariance matrix of the two sub-filters and the state estimation value of the main filter to obtain a global optimal state estimation value, and feeding back the global optimal state estimation value to the two sub-filters to reset the sub-filters.

the integrated navigation method comprises the following steps:

A. establishing state equation of unmanned ship integrated navigation system

Selecting a northeast geographic coordinate system as a navigation coordinate system, and considering error factors of a strapdown inertial navigation system, an accelerometer and a gyroscope by using a state equation, wherein the state equation of the integrated navigation system is as follows:

Wherein the content of the first and second substances,The state estimation value of the integrated navigation system is shown as F, the state transition matrix of the integrated navigation system is shown as X, the state variable of the integrated navigation system is shown as G, the control matrix of the integrated navigation system is shown as W, and the white noise matrix of the integrated navigation system is shown as W.

The state variable X of the integrated navigation system is:

in the formula (I), the compound is shown in the specification,the attitude error angles in the east, north and sky directions of the unmanned ship are respectively; delta V_E、δV_N、δV_UThe speed errors in the east, north and sky directions of the unmanned ship are respectively; delta lambda, delta L and delta h are errors of the latitude, longitude and altitude of the unmanned ship respectively; epsilon_bx、ε_by、ε_bzrespectively the ordinary drift of the gyroscope in east, north and sky directions under the carrier system; epsilon_rx、ε_ry、ε_rzThe slow-changing drifts of the gyroscope in east, north and sky directions under the carrier system are respectively; v_x、▽_y、▽_zThe carrier is the normal random error of the accelerometer in east, north and sky directions.

The white noise matrix W of the integrated navigation system is as follows:

W＝[ω_gx,ω_gy,ω_gz,ω_rx,ω_ry,ω_rz,ω_ax,ω_ay,ω_az]^T

wherein, ω is_gx、ω_gy、ω_gzwhite noise in the east, north and sky directions of the gyroscope respectively; omega_rx、ω_ry、ω_rzWhite noise is driven by the first-order Markov process in the east, north and sky directions of the gyroscope respectively; omega_ax、ω_ay、ω_azWhite noise is driven for the first order markov process for the east, north, and sky directions of the accelerometer.

The error model of the gyroscope is:

ε＝ε_b+ε_r+ω_g

in the formula, epsilon_bis a random constant; epsilon_rRandom noise, epsilon, for a first order Markov process_rSatisfies the following formula:

T_gfor time, ω, of gyroscope_rWhite noise is driven for the first order markov process of the gyroscope.

The error model of the accelerometer is:

▽＝▽_a

Wherein ^ is the error of the accelerometer,T_aFor time, omega, of accelerometer correlation_adriving white noise for the first order Markov process of the accelerometer +_aIs the velocity error of the accelerometer.

the state transition matrix of the integrated navigation system is as follows:

In the formula, F_Nthe system matrix of basic navigation parameters is 9 rows and 9 columns, and the specific steps are as follows:

F_N(2,7)＝-ω_ie sinL

F_N(4,2)＝-f_U

F_N(5,1)＝f_U

F_N(5,3)＝-f_E

F_N(6,1)＝-f_N

F_N(6,2)＝-f_E

F_N(6,7)＝-2V_Eω_ie sinL

F_N(9,6)＝1

the remaining elements are 0.

Wherein ω is_ieis the angular rate of rotation of the earth, f_E、f_UThe oblateness of the earth, R, in the east and the sky directions respectively_Eis the equatorial radius of the earth, R_MA principal radius of curvature in a meridian at any point on the surface of the earth; r_Nthe main curvature radius of any point on the earth surface along the unitary-mortise circle; l is longitude.

F_M＝diag{0，0,0,-1/T_rx,-1/T_ry,-1/T_rz,-1/T_ax,-1/T_ay,-1/T_az}

In the formula:

θ、Psi is roll angle, pitch angle and course angle of the unmanned ship respectively.

The control matrix of the integrated navigation system is as follows:

B. designing SINS/GPS sub-filter

The SINS/GPS sub-filter adopts a position and speed combined mode, and the measurement equation of the position and the speed of the SINS/GPS sub-filter is established as follows:

Z₁＝HX₁+V₁

Z₁As a measure of the SINS/GPS sub-filter, H₁Is a measurement matrix of SINS/GPS sub-filters, V₁The measured noise of the SINS/GPS sub-filter.

the speed information of the SINS is:

The speed information of the GPS is:

the position information of the SINS is as follows:

the position information of the GPS is:

in the formula, v_IE、v_IN、v_IURespectively velocity information of the SINS in east, north and sky directions; v. of_GE、v_GN、v_GUrespectively speed information of east, north and sky directions of the GPS; lambda [ alpha ]_I、L_I、h_IRespectively position information of the SINS in east, north and sky directions; lambda [ alpha ]_G、L_G、h_GPosition information of east, north and sky directions of the GPS respectively; v. of_N、v_E、v_UThe speed truth values of the directions of the unmanned ship east, north and sky are respectively; lambda [ alpha ]_t、L_t、h_tPosition truth values of latitude, longitude and altitude of the unmanned ship are respectively; delta v_E、δv_N、δv_UAre respectively east, north and south of SINS,speed error in the direction of the day; m_N、M_E、M_UThe speed errors in the east, north and sky directions of the GPS are respectively; δ λ, δ L, δ h are errors of latitude, longitude, altitude of SINS respectively, N_N、N_E、N_Uthe latitude, longitude and altitude errors of the GPS are respectively.

taking the velocity difference value of the SINS and the GPS as the measurement value of the SINS/GPS sub-filter, and defining a velocity measurement equation as follows:

in the formula:

Z_VAs a measure of the SINS/GPS sub-filter velocity information, H_Vis a measurement matrix, V, of SINS/GPS sub-filter velocity information_Vmeasurement noise, O, for SINS/GPS sub-filter velocity information_3×3zero matrix of 3 rows and 3 columns, O_3×12A zero matrix of 3 rows and 12 columns.

taking the position difference value of the SINS and the GPS as the measurement value of the SINS/GPS sub-filter, and defining a position measurement equation as follows:

in the formula:

V_p＝[N_N,N_E,N_U]^T

R_M＝R_e(1-2f+3fsin²L)

R_N＝R_e(1+fsin²L)

In the formula: z_PAs a measure of the SINS/GPS sub-filter velocity information, H_Pfor SINS/GPS sub-filterMeasurement matrix, V, of wave filter velocity information_PMeasurement noise, V, for SINS/GPS sub-filter velocity information_Pprocessing as white noise; r_eIs the equatorial radius of the earth; taking 6378137 m; f is the oblateness of the earth, and 1/298.257 is taken; o is_3×6A zero matrix with 3 rows and 6 columns; o is_3×9a zero matrix of 3 rows and 9 columns.

The combined mode of speed and position is adopted to establish a measurement equation of the SINS/GPS sub-filter as follows:

C. designing SINS/Compass sub-filters

The measurement equation for establishing the attitude of the unmanned ship integrated navigation SINS/Compass sub-filter is as follows:

Z₂＝H₂X+V₂

In the formula, Z₂Measured values for the SINS/Compass sub-filters, H₂Is a measurement matrix of SINS/Compass sub-filters, V₂Is the measurement noise of SINS/Compass sub-filter.

The posture information of the SINS is as follows:

compass pose information is:

In the formula (I), the compound is shown in the specification,respectively representing the posture information of the east, north and sky directions of the SINS;posture information of east, north and sky of Compass respectively;posture truth values of the unmanned ship in east, north and sky directions are respectively;respectively representing attitude errors in east, north and sky directions of the SINS; delta alpha_E、δα_N、δα_UThe attitude errors in east, north and sky directions of Compass, respectively.

And taking the difference value of the attitude information output by the SINS and the Compass as the measured value of the SINS/Compass sub-filter, wherein the observation equation is as follows:

In the formula, H₂＝[I_3×3,0_3×15]，I_3×3Is an identity matrix of 3 rows and 3 columns, O_3×15is a 3-row 15-column identity matrix, V₂＝[δα_c,δβ_c,δγ_c]^Tis the observed white noise with a mean of zero.

D. Process for implementing adaptive Federal Kalman Filter

the SINS/GPS sub-filter and the SINS/Compass sub-filter operate in parallel, the SINS, GPS and Compass data are subjected to distributed processing, and information fusion is carried out in the main filter. And the estimation error covariance arrays and the state estimation values of the two sub-filters are sent to the main filter, and are subjected to information fusion with the estimation value of the main filter to obtain the global optimal estimation. And feeding back the global optimal estimation to the sub-filters according to an information distribution principle to finish primary filtering, and resetting the estimation values of the two sub-filters.

The information fusion process of the self-adaptive Federal Kalman filter is carried out in the main filter, and comprises four steps of information distribution, time updating, measurement updating and information fusion, and the method comprises the following specific steps:

D1, information distribution

And distributing the global state estimation, the error variance matrix and the noise information to two sub-filters and a main filter, wherein the distribution principle is as follows:

wherein i-1 and 2 represent the SINS/GPS sub-filter and SINS/Compass sub-filter, respectively, and P_i,k-1Is the estimated mean square error covariance matrix, P, at the time of sub-filter k-1_g,k-1Estimating mean square error covariance matrix, Q, for global optimum at time k-1_i,k-1the system noise variance matrix, Q, at the moment of sub-filter noise k-1_g,k-1The system noise variance matrix at the moment of main filter noise k-1,For the state estimate at time instant k-1 of the sub-filter,Is the state estimate at the moment of the main filter k-1.

information distribution factor beta_iSatisfy the principle of information conservation, i.e.

no information is distributed in the main filter, and state estimation and time updating of covariance matrix are performed, i.e. P_m,k＝0。

D2, time update

time updating is independently carried out between the two sub-filters, and state prediction vectors and a one-step prediction mean square error covariance matrix of the two sub-filters are respectively obtained:

wherein i-1 and 2 represent the SINS/GPS sub-filter and SINS/Compass sub-filter, respectively, and P_i,k|k-1The mean square error covariance matrix is predicted for one step of the sub-filters,for predicting the state of the sub-filters, phi_i,k|k-1is a one-step transfer matrix, Γ, from sub-filter time k-1 to time k_i,k-1The array is driven by noise.

D3, measurement update

the two sub-filters receive the measurement information and independently perform a measurement updating process, respectively obtain state estimation values, filter gains and estimation mean square error covariance matrixes of the two sub-filters, and transmit the state estimation values, the filter gains and the estimation mean square error covariance matrixes to the main filter:

Wherein i-1 and 2 represent SINS/GPS sub-filter and SINS/Compass sub-filter respectively,For state estimation at sub-filter time K, K_i,kis the filter gain at time k of the sub-filter, P_i,kto estimate the mean square error covariance matrix, Z_i,kAs a measure of the sub-filter at time k, H_i,kMeasurement matrix of sub-filter k time, R_i,kand a measurement noise variance matrix at the moment k of the sub-filter, I unit matrix.

The measurement updating process is only carried out in the sub-filter, and the time updating process is only carried out in the main filter.

WhereinFor the state estimate at the moment of the main filter k-1,the vector is predicted for the state at the moment of the main filter k-1.

D4, information fusion

And fusing the state estimation information of the two sub-filters and the state estimation of the main filter to obtain global state estimation information:

in the formula, P_g,kEstimating the mean square error covariance matrix, P, for a global optimum at time k_1,kis an estimated mean square error covariance matrix, P, at time k of the SINS/GPS sub-filter_2,kis an estimated mean square error covariance matrix, P, at time k of the SINS/Compass sub-filter_m,kIs the estimated mean squared error covariance matrix at the moment k of the main filter,Is a global optimum state estimate for time k.

D5 calculating adaptive information distribution factor

The optimal adaptive factor satisfies the following conditions:

The optimal adaptation factor is expressed as follows:

The deviation of the geometric state vector from the predicted state vector is:

Wherein the geometric state vectorcomprises the following steps:

in the formula (E)_i,kIs Z_i,kThe equivalent weight matrix of (2).

Wherein the content of the first and second substances,To predict the state vector, P_i,k|k-1To predict the estimated covariance matrix of the state vector,Is a theoretical covariance matrix of the predicted state vector.

When the estimator is based on the predicted state vector at the current time, the estimated covariance matrix of the predicted state vector is as follows:

Optimal adaptive factor alpha_kis less than 1, expressed as:

In the formula:

The estimated covariance matrix of the optimal adaptation factor based on the prediction state vector is then expressed as:

The adaptation factor represented by the above equation is similar to the adaptation factor constructed from the difference between the predicted state of the model and the measured estimated state.

The adaptation factor is expressed as a prediction state vector in the form:

Wherein c is a constant,based on prediction of state vectorsthe statistics of the structure are expressed as:

The method for calculating the optimal adaptive factor based on the prediction state vector comprises the following steps of solving the adaptive information distribution factor obtained from the adaptive Federal Kalman filter as follows:

Wherein b is a constant and is 0.85-1.0.

In order to ensure that the self-adaptive information distribution factors meet the information conservation law, the self-adaptive information distribution factors are subjected to normalization processing. The adaptive information allocation factor is normalized to:

In the formula (II), beta'_i,kand distributing factors for the adaptive information of the normalized sub-filters at the k moment.

Compared with the prior art, the invention has the following beneficial effects:

1. The invention provides an unmanned ship SINS/GPS/Compass combined navigation system, which applies an error model and an observation model of unmanned ship SINS/GPS/Compass combined navigation, adopts Federal Kalman filtering, reduces fault interference among subsystems and improves the reliability and stability of the unmanned ship navigation system.

2. According to the invention, on the basis of the Federal Kalman filtering, according to a calculation method of an optimal adaptive factor of an estimated mean square error covariance matrix of a predicted state vector, an information distribution factor of a sub-filter of the Federal Kalman filtering is designed, and an adaptive Federal Kalman filtering algorithm is provided.

3. The method adopts a method for calculating the optimal adaptive factor of the estimated mean square error covariance matrix based on the predicted state vector to select the information distribution factor of the Federal filter, and has higher reliability compared with a method for calculating the optimal adaptive factor of the predicted residual error.

Drawings

FIG. 1 is a schematic flow diagram of the present invention.

FIG. 2 is a waveform diagram of EKF and AEKF simulation of latitude error.

FIG. 3 is a waveform diagram of EKF, AEKF simulation of longitude errors.

FIG. 4 is a waveform of EKF, AEKF simulation of altitude error.

FIG. 5 is a waveform of EKF, AEKF simulation of east-direction velocity error.

FIG. 6 is a waveform of EKF, AEKF simulation of north direction velocity error.

FIG. 7 is a waveform of EKF and AEKF simulation of the errors in the speed in the direction of the day.

FIG. 8 is a waveform of EKF and AEKF simulations of east attitude errors.

FIG. 9 is a waveform of EKF and AEKF simulation of north attitude error.

FIG. 10 is a waveform of EKF and AEKF simulations of attitude errors.

Detailed Description

The invention is further described below with reference to the accompanying drawings.

The adaptive Federal Kalman filter is improved based on the existing Federal Kalman filter, because the sub-filter of the Federal Kalman filter is used for adjusting the weight matrix of the state estimation value, and the adaptive Federal Kalman filter is used for adjusting the covariance matrix of the state estimation value, the correction parameters of the two are equivalent.

If the information distribution factor of the Federal Kalman filter is represented by the adaptive factor of the adaptive Federal Kalman filter, the adaptivity of the Federal Kalman filter can be improved, and the equivalence between the two is proved below.

Setting the filtering estimation value of the sub filter, the filtering estimation value of the main filter and the global state estimation to be X respectively_i,k、X_m,k、X_g,kThe corresponding weight matrix is sigma_i,k、∑_m,k、∑_g,kthe corresponding covariance matrices are each P_i,k、P_m,k、P_g,k。

the self-adaptive Federal Kalman filtering enables filtering parameters to be self-adaptively adjusted through self-adaptive factors, and therefore the optimal filtering effect is obtained.

by using the least square principle, the extreme value principle of the self-adaptive Federal Kalman filtering is as follows:

wherein alpha is_kas an adaptation factor, V_kAnderror vectors of the state prediction information and observation vector, respectively, and the corresponding covariance matrix is sigma_kAnd

Deriving an extremum function from the error variances of the predicted state vector and the observed vector as:

Wherein λ_kis the lagrange multiplier.

the adaptive federal kalman filter solution is then:

Wherein the content of the first and second substances,the gain matrix for adaptive federal kalman filtering is:

Because:And is

Therefore:

Writing the gain matrix of the Federal Kalman Filter to the following equation:

If α is_k＝β_iThen G is_ikandequivalently, the sub-filters illustrating the federal kalman filter and the adaptive federal kalman filter are equivalent in form.

according to the above proof, the information distribution factor of the Federal Kalman filter can be calculated by the optimal adaptive factor calculation method. Therefore, the self-adaptive Federal Kalman filtering algorithm is provided, the self-adaptability of the sub-filters can be increased, and the filtering precision is improved.

In order to illustrate the effectiveness and feasibility of the invention, Matlab simulation is carried out under an SINS error model, and the adaptive Federal Kalman filtering algorithm provided by the invention is verified. Assume that the initial position of the unmanned ship is 121.4 ° east longitude and 39.0 ° north latitude. The error parameters of the sensor are set as: the constant drift of the gyroscope is pi/180 degrees/h, the slow-changing drift of the gyroscope is pi/180 degrees/h, the bias of the accelerometer is 1g, the attitude error of the three-dimensional electronic compass is 0.5 degrees, the speed measurement error of the GPS is 0.1m/s, and the position error is 10 m.

Table 1 shows comparison of simulation error parameter ranges between the federal kalman filter method (EKF) and the adaptive federal kalman filter method (AEKF), and it can be seen from the figure that errors of the adaptive federal kalman filter Algorithm (AEKF) can be converged to a smaller range than those of the federal kalman filter algorithm (EKF), when the EKF algorithm can make errors larger, the AEKF algorithm can still reduce the errors, the control errors are in a smaller range, and when the EKF algorithm obtains smaller errors, the errors of the AEKF algorithm can also be controlled to a smaller range. FIG. 1 shows a diagram of the SINS/GPS/Compass federal filter architecture, from which the data processing from the filter and the main filter can be seen. Fig. 2 shows the waveform diagrams of EKF and AEKF simulation of latitude, longitude and altitude errors, and it can be seen from the diagram that the latitude, longitude and altitude errors of the AEKF algorithm can be controlled in a smaller range, which is better than the EKF algorithm. Fig. 3 shows the EKF and AEKF simulation waveforms of the east, north and sky speed errors, and it can be seen from the waveform that the east, north and sky speed errors of the AEKF algorithm can be controlled in a smaller range, and the effect is better compared with the EKF algorithm. Fig. 4 shows the EKF and AEKF simulation oscillograms of the attitude errors in the east, north and sky directions, and it can be seen from the graphs that the range of the attitude errors in the sky direction of the EKF algorithm is too large, and the AEKF algorithm better controls the errors and controls the errors within a reasonable range. It can be concluded that: the effectiveness and superiority of the algorithm scheme provided by the invention are verified through the experiments.

TABLE 1 EKF and AEKF simulation error parameter comparison

	error in latitude	longitude (G)	Height
				EKF	-1.885～1.939	-0.710～0.618	-3.857～4.010
AEKF	-1.840～1.435	-0.714～0.616	-0.592～0.488
					East velocity error	error in north direction velocity	Error in speed in the direction of the sky
EKF	-0.693～1.201	-3.693～14.896	-0.505～0.745
				AEKF	-0.693～1.201	-0.559～1.226	-0.377～0.586
	East attitude error	North attitude error	Attitude error in the sky
				EKF	-0.209～0.266	-0.210～0.268	-3690.023～4708.490
AEKF	-0.224～0.246	-0.210～0.267	-51.710～100.228

The present invention is not limited to the embodiment, and any equivalent idea or change within the technical scope of the present invention is to be regarded as the protection scope of the present invention.