Integrated navigation fault-tolerant method based on self-adaptive federal strong tracking filtering

文档序号:1462905 发布日期:2020-02-21 浏览:6次 中文

阅读说明:本技术 一种基于自适应联邦强跟踪滤波的组合导航容错方法 (Integrated navigation fault-tolerant method based on self-adaptive federal strong tracking filtering ) 是由 熊海良 麦珍珍 卞若晨 王广渊 胡昌武 于 2019-11-21 设计创作,主要内容包括:本发明公开了一种基于自适应联邦强跟踪滤波的组合导航容错方法,包括如下步骤:建立组合导航系统状态模型;构建故障检测函数进行检测;构建自适应强跟踪滤波算法进行状态估计;将无故障子滤波器的输出数据送至主滤波器进行自适应信息融合,得到全局状态估计值;将全局状态估计值反馈给无故障子系统,对于存在故障的子系统,需要重置子系统信息;将获得的全局状态估计值校正惯性导航系统输出的导航参数。本发明公开的方法不仅能提高系统的滤波精度,还能提高滤波器对系统状态发生突变时的跟踪能力,可以对故障系统进行检测和隔离,提升融合结果的精度和可靠性,从而增强组合导航系统的鲁棒性,本方法简单,易于实现。(The invention discloses a combined navigation fault-tolerant method based on self-adaptive federal strong tracking filtering, which comprises the following steps: establishing a state model of the integrated navigation system; constructing a fault detection function for detection; constructing a self-adaptive strong tracking filtering algorithm for state estimation; sending the output data of the fault-free sub-filter to a main filter for self-adaptive information fusion to obtain a global state estimation value; feeding back the global state estimated value to a fault-free subsystem, and resetting subsystem information for the subsystem with the fault; and correcting the navigation parameters output by the inertial navigation system according to the obtained global state estimated value. The method disclosed by the invention can improve the filtering precision of the system, can also improve the tracking capability of the filter when the system state is suddenly changed, can detect and isolate a fault system, and improves the precision and reliability of a fusion result, thereby enhancing the robustness of the combined navigation system.)

1. A combined navigation fault-tolerant method based on self-adaptive federal strong tracking filtering is characterized by comprising the following steps:

step 1: selecting state quantity and observed quantity by taking SINS as a reference system, and establishing a state model of an integrated navigation system and a measurement equation of an SINS/GPS and an SINS/DVL subsystem;

step 2: constructing a fault detection function for detection;

and step 3: constructing a self-adaptive strong tracking filtering algorithm for state estimation;

and 4, step 4: sending the output data of the fault-free sub-filter to a main filter for self-adaptive information fusion to obtain a global state estimation value;

and 5: feeding back the global state estimated value to a fault-free subsystem, and resetting subsystem information for the subsystem with the fault;

step 6: and correcting the navigation parameters output by the inertial navigation system according to the obtained global state estimated value.

2. The combined navigation fault-tolerant method based on the adaptive federal strong tracking filtering according to claim 1, wherein the step 1 is specifically as follows:

establishing a state model of the integrated navigation system, which is as follows:

Figure RE-FDA0002312575800000011

wherein X (t) is system state vector, F (t) is state transition matrix, and W (t) is system white noise;

Figure RE-FDA0002312575800000012

φENUrespectively representing the misalignment angles of the east direction, the north direction and the sky direction in a navigation coordinate system of the strapdown inertial navigation system;

δvE,δvN,δvUrespectively representing the speed errors of the strapdown inertial navigation system in the east direction, the north direction and the sky direction;

δ L, δ λ and δ h respectively represent latitude error, longitude error and altitude error of the strapdown inertial navigation system;

εrxryrzrespectively representing the gyro drift of X, Y, Z three directions in a strapdown inertial navigation system carrier coordinate system;

Figure RE-FDA0002312575800000013

the SINS/GPS subsystem selects position and velocity as the measurement, i.e.

Figure RE-FDA0002312575800000014

In the formula, uINSEINSNINSU,LINSINS,hINSEast-direction speed, north-direction speed, sky-direction speed, latitude, longitude and altitude which are respectively solved by inertial navigation;

υGPSEGPSNGPSU,LGPSGPS,hGPSthe speed, latitude, longitude and altitude of the east direction, the north direction, the sky direction, measured by the GPS respectively;

H1(t) is a measurement matrix, and,

Figure RE-FDA0002312575800000021

V1(t) position and speed error noise of the GPS receiver in the navigation coordinate system;

the SINS/DVL subsystem selects speed as the measurement, i.e.

Figure RE-FDA0002312575800000022

In the formula, uDVLEDVLNDVLUEast speed, north speed and sky speed measured by Doppler respectively;

H2(t) is a measurement matrix, and,

H2=|03×3diag|1 1 1| 03×9|;

V2(t) is the velocity error noise of the DVL in the navigation coordinate system;

respectively carrying out discretization treatment to obtain:

Figure RE-FDA0002312575800000023

in the formula, Fi,k|k-1,Hik,Wi,k-1,VikThe state transition array, the measurement array, the system noise array and the measurement noise array of the ith sub-filter are respectively.

3. The combined navigation fault-tolerant method based on the adaptive federal strong tracking filtering according to claim 1, wherein the step 2 is specifically as follows:

defining fault detection functions

Figure RE-FDA0002312575800000024

Wherein: e.g. of the typekIs a residual error, i.e.E[ek]=μ,E{[ek-μ][ek-μ]T}=Σk

γkObeying x degree of freedom of m2Distribution, i.e. gammak~χ2(m), m is a measurement zkThe dimension of (a);

the failure determination criterion is:

γk>TDand the occurrence of a fault,

γk≤TDno fault occurs;

wherein, TDIs a threshold value, TD=f(χ2,PFA/m),PFAThe false alarm rate is.

4. The combined navigation fault-tolerant method based on the adaptive federal strong tracking filtering according to claim 1, wherein the step 3 is specifically as follows:

the main process of the self-adaptive strong tracking filtering algorithm comprises the steps of solving fading factors, time updating and measurement updating: estimating the covariance of the residual sequence by using a windowing method, and adaptively adjusting a measurement noise array to obtain an improved fading factor; solving one-step predictions of state using time updates

Figure RE-FDA0002312575800000031

① solving for adaptive fading factor lambdai,k

Calculating a residual error:

Figure RE-FDA0002312575800000033

estimate of residual covariance:

Figure RE-FDA0002312575800000034

theoretical value of residual covariance:

Figure RE-FDA0002312575800000035

Figure RE-FDA0002312575800000036

Figure RE-FDA0002312575800000037

Figure RE-FDA0002312575800000038

Figure RE-FDA0002312575800000039

wherein Tr (-) represents tracing the matrix; z is a radical ofi,kFor true measurement, Ri,kMeasuring noise variance matrix, N is sampling number or window length, ξ is weakening factor, generally ξ is more than or equal to 1;

② time update:

one-step prediction equation of state:

Figure RE-FDA00023125758000000310

one-step prediction error covariance equation:

Figure RE-FDA0002312575800000041

③ measurement update:

filter gain equation:

the state estimation calculation equation:

Figure RE-FDA0002312575800000043

estimate error covariance equation Pi,k=[I-Ki,kHi,k]Pi,k|k-1.;

In the formula, λi,kIs an evanescent factor, λi,kWhen the prediction error covariance is more than 1, the covariance of the one-step prediction error can be adaptively adjustedPi,k|k-1Strong tracking filtering; when lambda isi,kAnd when the filtering rate is less than or equal to 1, standard KF filtering is performed.

5. The combined navigation fault-tolerant method based on the adaptive federal strong tracking filtering according to claim 1, wherein the step 4 is specifically as follows:

the output data of the fault-free sub-filter is sent to the main filter for self-adaptive information fusion to obtain a global state estimation value,

the fusion mode is as follows:

in the formula (I), the compound is shown in the specification,for global state estimation, PgEstimating covariance matrices for global states βiIn order to be the weighting coefficients,

Figure RE-FDA0002312575800000046

if the sub-filters are detected to be out of order, the data of the sub-systems with faults are isolated in the fusion process, and the accuracy of the fusion result is guaranteed.

6. The combined navigation fault-tolerant method based on the adaptive federal strong tracking filtering according to claim 1, wherein the step 5 is specifically as follows:

and feeding back the global estimated value to a fault-free subsystem, wherein the information distribution principle is as follows:

Pi,k=βi -1Pg

for subsystem j with a failure, the subsystem information needs to be reset, at βj=1。

7. The combined navigation fault-tolerant method based on the adaptive federal strong tracking filtering according to claim 1, wherein the step 6 is as follows: and feeding back the global error estimation value to the inertial navigation system, and subtracting the global error estimation value from the navigation parameter output by the inertial navigation system to obtain the final estimation value of the navigation parameter.

Technical Field

The invention belongs to the technical field of integrated navigation, and particularly relates to an integrated navigation fault-tolerant method based on self-adaptive federal strong tracking filtering.

Background

With the progress of science and technology and the development of times, the demands for high-precision and high-reliability navigation positioning are continuously increased in both military fields and civil fields. A Global Navigation Satellite System (GNSS) is a main means of outdoor high-precision positioning and has the characteristics of high long-term measurement precision, all-weather work and the like, but in complex areas such as forests, urban canyons and the like, satellite signals are easy to be shielded and interfered, so that the GNSS positioning equipment cannot normally output navigation parameters. A Strapdown Inertial Navigation System (SINS) is an autonomous navigation system that can provide accurate relative position information in a short time, but is limited by error accumulation and is not suitable for long-time navigation. The doppler log (DVL) obtains velocity information by measuring a frequency shift using a doppler effect, has a strong real-time property and a certain autonomy, but has interference noise and deviation when measuring a velocity because of its factors based on an acoustic principle and installation. In consideration of the problems of poor positioning accuracy, poor stability and the like of a single system, two or more navigation systems are combined by a certain method to form a combined navigation system, and the advantages of each sub-navigation system are made full of advantages and disadvantages, so that the accuracy of navigation positioning is improved. Therefore, integrated navigation systems have become a trend.

For a combined navigation system, under the condition of certain system hardware performance, an advanced filtering algorithm is an effective way for improving the precision, the real-time performance and the reliability of the combined navigation system and realizing the cooperative transcendence. Kalman Filtering (KF) is the most common estimation technique for integrating the signals output by the sensors to produce an optimal solution, but can only be used in linear systems. Extended Kalman Filtering (EKF), which simply linearizes all nonlinear models to the first order, is widely used to solve the nonlinear state estimation problem. However, the EKF needs Taylor series expansion on an equation to solve the Jacobian matrix, the calculation amount is large, and the linear approximation precision on the nonlinear function is not high. The UKF is the combination of UT transformation and KF, and the core idea is to approximate the posterior probability density function of the nonlinear system state by using a group of Sigma points through UT transformation, and the UKF is simple to realize, high in filtering precision and good in convergence. However, when the system state equations or the measurement equations have linear characteristics, the standard UKF has a large amount of redundant computation in the filtering solution. In addition, the UKF requires that the prior statistical information of the system process model and the measurement model is accurately known, and the uncertainty of the system model can cause the filtering precision of the UKF to be reduced and even to be diverged. The Strong Tracking Filtering (STF) algorithm proposed by Zhonghuadong et al increases prediction error covariance by introducing an evanescent factor, and adjusts a corresponding gain matrix on line, so that effective information is completely extracted in the filtering process, a filter tracks the state of a system in real time, and the robustness is high. However, in the conventional strong tracking algorithm, the selection of the fading factor parameter is usually based on an empirical value, and under a normal condition of the system, the strong tracking has a certain false judgment probability. The above two factors affect the accuracy of filtering.

The optimal combination of the integrated navigation system is carried out by using a filtering technology, and generally, two modes are adopted: centralized fusion and decentralized fusion. The centralized fusion needs to send the measurement information of each sensor to a central station for centralized processing, so that the calculation burden is heavy, the real-time operation of filtering is not facilitated, and the fault tolerance performance is poor. The federate kalman filtering technique among the distributed filtering techniques is widely applied due to the flexibility of design, small calculated amount and good fault tolerance. In the fusion process, a large number of inverse matrixes of covariance matrixes of local state estimation values and global state estimation values need to be calculated by a main filter of the FKF, and the calculation complexity is high; especially when the dimension of the selected integrated navigation system state is high, the calculation amount is increased sharply. Moreover, when a large error occurs in observation of a certain subsystem or filtering fails, a large error also occurs in the fusion result of the main filter, thereby affecting the filtering precision of the system. Therefore, the self-adaptive federal filtering structure with error detection and fault tolerance is designed, the system calculation complexity is reduced, the positioning performance is improved, and the practical application value is achieved.

Disclosure of Invention

In order to solve the technical problems, the invention provides a combined navigation fault-tolerant method based on self-adaptive federal strong tracking filtering, so as to achieve the purposes of improving the filtering precision, improving the tracking capability of a filter when the system state is suddenly changed, and having the functions of error detection and fault tolerance.

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

a combined navigation fault-tolerant method based on self-adaptive federal strong tracking filtering comprises the following steps:

step 1: selecting state quantity and observed quantity by taking SINS as a reference system, and establishing a state model of an integrated navigation system and a measurement equation of an SINS/GPS and an SINS/DVL subsystem;

step 2: constructing a fault detection function for detection;

and step 3: constructing a self-adaptive strong tracking filtering algorithm for state estimation;

and 4, step 4: sending the output data of the fault-free sub-filter to a main filter for self-adaptive information fusion to obtain a global state estimation value;

and 5: feeding back the global state estimated value to a fault-free subsystem, and resetting subsystem information for the subsystem with the fault;

step 6: and correcting the navigation parameters output by the inertial navigation system according to the obtained estimated value of the global error.

In the above scheme, the step 1 is specifically as follows:

establishing a state model of the integrated navigation system, which is as follows:

Figure RE-GDA0002312575810000031

wherein X (t) is system state vector, F (t) is state transition matrix, and W (t) is system white noise;

Figure RE-GDA0002312575810000032

φENUrespectively representing the misalignment angles of the east direction, the north direction and the sky direction in a navigation coordinate system of the strapdown inertial navigation system;

δvE,δvN,δvUrespectively representing the speed errors of the strapdown inertial navigation system in the east direction, the north direction and the sky direction;

δ L, δ λ and δ h respectively represent latitude error, longitude error and altitude error of the strapdown inertial navigation system;

εrxryrzrespectively representing the gyro drift of X, Y, Z three directions in a strapdown inertial navigation system carrier coordinate system;

Figure RE-GDA0002312575810000033

respectively representing the zero offset of the accelerometer in X, Y, Z three directions in a carrier coordinate system of the strapdown inertial navigation system;

the SINS/GPS subsystem selects position and velocity as the measurement, i.e.

Figure RE-GDA0002312575810000034

In the formula, uINSEINSNINSU,LINSINS,hINSEast-direction speed, north-direction speed, sky-direction speed, latitude, longitude and altitude which are respectively solved by inertial navigation;

υGPSEGPSNGPSU,LGPSGPS,hGPSthe speed, latitude, longitude and altitude of the east direction, the north direction, the sky direction, measured by the GPS respectively;

H1(t) is a measurement matrix, and,

Figure RE-GDA0002312575810000035

V1(t) position and speed error noise of the GPS receiver in the navigation coordinate system;

the SINS/DVL subsystem selects speed as the measurement, i.e.

Figure RE-GDA0002312575810000036

In the formula, uDVLEDVLNDVLUEast speed, north speed and sky speed measured by Doppler respectively;

H2(t) is a measurement matrix, and,

H2=|03×3diag|1 1 1|03×9|;

V2(t) is the velocity error noise of the DVL in the navigation coordinate system;

respectively carrying out discretization treatment to obtain:

Figure RE-GDA0002312575810000041

in the formula, Fi,k|k-1,Hik,Wi,k-1,VikThe state transition array, the measurement array, the system noise array and the measurement noise array of the ith sub-filter are respectively.

In the above scheme, the step 2 is specifically as follows:

defining fault detection functions

Figure RE-GDA0002312575810000042

Wherein: e.g. of the typekIs a residual error, i.e.E[ek]=μ,E{[ek-μ][ek-μ]T}=Σk

γkObeying x degree of freedom of m2Distribution, i.e. gammak~χ2(m), m is a measurement zkThe dimension of (a);

the failure determination criterion is:

γk>TDand the occurrence of a fault,

γk≤TDno fault occurs;

wherein, TDIs a threshold value, TD=f(χ2,PFA/m),PFAThe false alarm rate is.

In the above scheme, the step 3 is specifically as follows:

the main process of the self-adaptive strong tracking filtering algorithm comprises the steps of solving fading factors, time updating and measurement updating: estimating the covariance of the residual sequence by using a windowing method, and adaptively adjusting a measurement noise array to obtain an improved fading factor; solving one-step predictions of state using time updates

Figure RE-GDA0002312575810000044

One-step prediction of error covariance P for states using an evanescent factork|k-1Adjusting; evaluating state estimates using measurement updatesAn estimate of the sum variance Pk(ii) a The specific process is as follows:

① solving for adaptive fading factor lambdai,k

Calculating a residual error:

Figure RE-GDA0002312575810000046

estimate of residual covariance:

theoretical value of residual covariance:

Figure RE-GDA0002312575810000052

Figure RE-GDA0002312575810000053

Figure RE-GDA0002312575810000056

wherein Tr (-) represents tracing the matrix; z is a radical ofi,kFor true measurement, Ri,kMeasuring noise variance matrix, N is sampling number or window length, ξ is weakening factor, generally ξ is more than or equal to 1;

② time update:

one-step prediction equation of state:

Figure RE-GDA0002312575810000057

one-step prediction error covariance equation:

Figure RE-GDA0002312575810000058

③ measurement update:

filter gain equation:

Figure RE-GDA0002312575810000059

the state estimation calculation equation:

Figure RE-GDA00023125758100000510

estimate error covariance equation Pi,k=[I-Ki,kHi,k]Pi,k|k-1.;

In the formula, λi,kIs an evanescent factor, λi,kWhen the prediction error is more than 1, the covariance P of the one-step prediction error can be adaptively adjustedi,k|k-1Strong tracking filtering; when lambda isi,kAnd when the filtering rate is less than or equal to 1, standard KF filtering is performed.

In the above scheme, the step 4 is specifically as follows:

sending the output data of the fault-free sub-filter to a main filter for self-adaptive information fusion to obtain a global state estimation value, wherein the fusion mode is as follows:

Figure RE-GDA00023125758100000511

in the formula (I), the compound is shown in the specification,

Figure RE-GDA0002312575810000061

for global state estimation, PgEstimating covariance matrices for global states βiIn order to be the weighting coefficients,

Figure RE-GDA0002312575810000062

if the sub-filters are detected to be out of order, the data of the sub-systems with faults are isolated in the fusion process, and the accuracy of the fusion result is guaranteed.

In the above scheme, the step 5 is specifically as follows:

and feeding back the global state estimated value to a fault-free subsystem, wherein the information distribution principle is as follows:

Figure RE-GDA0002312575810000063

for subsystem j with a failure, the subsystem information needs to be reset, at βj=1;

In the above scheme, the step 6 is specifically as follows: and feeding back the global error estimation value to the inertial navigation system, and subtracting the global error estimation value from the navigation parameter output by the inertial navigation system to obtain the final estimation value of the navigation parameter.

Through the technical scheme, the combined navigation fault-tolerant method based on the self-adaptive federal strong tracking filtering has the following beneficial effects:

firstly, detecting the filtering state of a subsystem and judging whether the system normally operates; calculating an improved fading factor and correcting an error covariance value of one-step state prediction by using a self-adaptive strong tracking filtering technology in a subsystem; in the fusion process of the main filter, the output data of the fault-free subsystem is subjected to self-adaptive fusion, the fault subsystem data is isolated, and the accuracy of the obtained global estimation is ensured; and correcting the navigation parameters output by the inertial navigation system by using the global optimal estimation value, feeding back the global estimation value to the fault-free subsystem, and resetting the fault subsystem. The invention can not only improve the filtering precision of the system, but also improve the tracking capability of the filter when the system state is suddenly changed, can detect and isolate a fault system, and improves the precision and reliability of a fusion result, thereby enhancing the robustness of the integrated navigation system.

Drawings

Fig. 1 is a schematic flow chart of a combined navigation fault-tolerant method based on adaptive federal strong tracking filtering disclosed by the invention.

Detailed Description

The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

The invention provides a combined navigation fault-tolerant method based on self-adaptive federal strong tracking filtering, which comprises the following specific embodiments:

as shown in fig. 1, a combined navigation fault-tolerant method based on adaptive federal strong tracking filtering includes the following steps:

step 1: selecting state quantity and observed quantity by taking SINS as a reference system, and establishing a state model of an integrated navigation system and a measurement equation of an SINS/GPS and an SINS/DVL subsystem;

establishing a state model of the integrated navigation system, which is as follows:

Figure RE-GDA0002312575810000071

wherein X (t) is system state vector, F (t) is state transition matrix, and W (t) is system white noise;

Figure RE-GDA0002312575810000072

φENUrespectively representing the misalignment angles of the east direction, the north direction and the sky direction in a navigation coordinate system of the strapdown inertial navigation system;

δvE,δvN,δvUrespectively representing the speed errors of the strapdown inertial navigation system in the east direction, the north direction and the sky direction;

δ L, δ λ and δ h respectively represent latitude error, longitude error and altitude error of the strapdown inertial navigation system;

εrxryrzrespectively representing the gyro drift of X, Y, Z three directions in a strapdown inertial navigation system carrier coordinate system;

Figure RE-GDA0002312575810000073

respectively representing the zero offset of the accelerometer in X, Y, Z three directions in a carrier coordinate system of the strapdown inertial navigation system;

the SINS/GPS subsystem selects position and velocity as the measurement, i.e.

Figure RE-GDA0002312575810000074

In the formula, uINSEINSNINSU,LINSINS,hINSEast-direction speed, north-direction speed, sky-direction speed, latitude, longitude and altitude which are respectively solved by inertial navigation;

υGPSEGPSNGPSU,LGPSGPS,hGPSthe speed, latitude, longitude and altitude of the east direction, the north direction, the sky direction, measured by the GPS respectively;

H1(t) is a measurement matrix, and,

Figure RE-GDA0002312575810000075

V1(t) position and speed error noise of the GPS receiver in the navigation coordinate system;

the SINS/DVL subsystem selects speed as the measurement, i.e.

Figure RE-GDA0002312575810000081

In the formula, uDVLEDVLNDVLUEast speed, north speed and sky speed measured by Doppler respectively;

H2(t) is a measurement matrix, and,

H2=|03×3diag|1 1 1|03×9|;

V2(t) is the velocity error noise of the DVL in the navigation coordinate system;

respectively carrying out discretization treatment to obtain:

Figure RE-GDA0002312575810000082

in the formula, Fi,k|k-1,Hik,Wi,k-1,VikThe state transition array, the measurement array, the system noise array and the measurement noise array of the ith sub-filter are respectively.

Step 2: constructing a fault detection function for detection;

defining fault detection functions

Figure RE-GDA0002312575810000083

Wherein: e.g. of the typekIs a residual error, i.e.

Figure RE-GDA0002312575810000084

E[ek]=μ,E{[ek-μ][ek-μ]T}=Σk

γkObeying x degree of freedom of m2Distribution, i.e. gammak~χ2(m), m is a measurement zkThe dimension of (a);

the failure determination criterion is:

γk>TDand the occurrence of a fault,

γk≤TDam, amA fault occurs;

wherein, TDIs a threshold value, TD=f(χ2,PFA/m),PFAThe false alarm rate is.

And step 3: constructing a self-adaptive strong tracking filtering algorithm for state estimation;

the main process of the self-adaptive strong tracking filtering algorithm comprises the steps of solving fading factors, time updating and measurement updating: estimating the covariance of the residual sequence by using a windowing method, and adaptively adjusting a measurement noise array to obtain an improved fading factor; solving one-step predictions of state using time updates

Figure RE-GDA0002312575810000085

One-step prediction of error covariance P for states using an evanescent factork|k-1Adjusting; evaluating state estimates using measurement updates

Figure RE-GDA0002312575810000091

An estimate of the sum variance Pk(ii) a The specific process is as follows:

① solving for adaptive fading factor lambdai,k

Calculating a residual error:

Figure RE-GDA0002312575810000092

estimate of residual covariance:

theoretical value of residual covariance:

Figure RE-GDA0002312575810000094

Figure RE-GDA0002312575810000095

Figure RE-GDA0002312575810000096

Figure RE-GDA0002312575810000098

wherein Tr (-) represents tracing the matrix; z is a radical ofi,kFor true measurement, Ri,kMeasuring noise variance matrix, N is sampling number or window length, ξ is weakening factor, generally ξ is more than or equal to 1;

② time update:

one-step prediction equation of state:

Figure RE-GDA0002312575810000099

one-step prediction error covariance equation:

Figure RE-GDA00023125758100000910

③ measurement update:

filter gain equation:

Figure RE-GDA00023125758100000911

the state estimation calculation equation:

Figure RE-GDA00023125758100000912

estimate error covariance equation Pi,k=[I-Ki,kHi,k]Pi,k|k-1.;

In the formula, λi,kIs an evanescent factor, λi,kWhen the prediction error is more than 1, the covariance P of the one-step prediction error can be adaptively adjustedi,k|k-1Strong tracking filtering; when lambda isi,kAnd when the filtering rate is less than or equal to 1, standard KF filtering is performed.

And 4, step 4: the output data of the fault-free sub-filter is sent to the main filter for self-adaptive information fusion to obtain a global state estimation value,

the fusion mode is as follows:

Figure RE-GDA0002312575810000101

in the formula (I), the compound is shown in the specification,

Figure RE-GDA0002312575810000102

for global state estimation, PgEstimating covariance matrices for global states βiIn order to be the weighting coefficients,

Figure RE-GDA0002312575810000103

if the sub-filters are detected to be out of order, the data of the sub-systems with faults are isolated in the fusion process, and the accuracy of the fusion result is guaranteed.

And 5: the global state estimate is fed back to the non-faulty subsystem,

the information distribution principle is as follows:

for subsystem j with a failure, the subsystem information needs to be reset, at βj=1;

Step 6: and correcting the navigation parameters output by the inertial navigation system by using the obtained estimated value of the global error: and (3) filtering by adopting an indirect method, taking the error as a state variable, obtaining an estimated value of the error through a multi-sensor fusion filtering system, then feeding back the estimated value to the inertial navigation system, and subtracting the estimated value of the global error from the navigation parameter output by the inertial navigation system to obtain the estimated value of the final navigation parameter.

The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

13页详细技术资料下载
上一篇:一种医用注射器针头装配设备
下一篇:物体跟踪系统

网友询问留言

已有0条留言

还没有人留言评论。精彩留言会获得点赞!

精彩留言,会给你点赞!