CN114741659B - Adaptive model on-line reconstruction robust filtering method, device and system - Google Patents

Adaptive model on-line reconstruction robust filtering method, device and system Download PDF

Info

Publication number
CN114741659B
CN114741659B CN202210448169.8A CN202210448169A CN114741659B CN 114741659 B CN114741659 B CN 114741659B CN 202210448169 A CN202210448169 A CN 202210448169A CN 114741659 B CN114741659 B CN 114741659B
Authority
CN
China
Prior art keywords
model
representing
state
probability
models
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210448169.8A
Other languages
Chinese (zh)
Other versions
CN114741659A (en
Inventor
秦晓辉
张润邦
秦兆博
胡满江
王晓伟
边有钢
徐彪
谢国涛
秦洪懋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Original Assignee
Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd filed Critical Jiangsu Jicui Qinglian Intelligent Control Technology Co ltd
Priority to CN202210448169.8A priority Critical patent/CN114741659B/en
Publication of CN114741659A publication Critical patent/CN114741659A/en
Application granted granted Critical
Publication of CN114741659B publication Critical patent/CN114741659B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Theoretical Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Operations Research (AREA)
  • Probability & Statistics with Applications (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Evolutionary Biology (AREA)
  • Computing Systems (AREA)
  • Feedback Control In General (AREA)
  • Navigation (AREA)

Abstract

The embodiment of the invention discloses a method, equipment and a system for reconstructing robust filtering on line by a self-adaptive model, wherein the method comprises the following steps: initializing a model set, a Markov probability transition matrix and a model probability of each model in the model set; obtaining an input state quantity and a state covariance matrix of each model; filtering and fusing data information acquired by the integrated navigation system by using a robust filter, and outputting state vectors corresponding to the models; updating the model probability of each model; and mixing the covariance matrix of each model, the state vector corresponding to each model and the updated model probability to obtain the state vector and the state covariance matrix of the carrier. In the invention, the robust filter is used for filtering and fusing the combined navigation system, so that the adverse effect caused by the measured outlier can be effectively resisted, and the total size of the interactive multi-model preset model set is reduced.

Description

Adaptive model online reconstruction robust filtering method, device and system
Technical Field
The invention relates to the field of information fusion, in particular to a method, equipment and a system for reconstructing robust filtering on line by using a self-adaptive model.
Background
The INS/DVL (inertial navigation system/Doppler velocimeter) combined navigation system can provide continuous navigation information for the autonomous underwater vehicle, and a reasonable and effective INS/DVL data fusion algorithm has important significance for improving navigation positioning accuracy.
At present, the INS/DVL integrated navigation system data fusion algorithm is mostly based on the traditional Kalman filtering framework. Theoretically, the kalman filter can obtain the optimal estimation of the fusion state only under the condition that the structural parameters and the noise statistical characteristic parameters of the random dynamic system are accurately known. However, the variable marine environment, such as the change of temperature, pressure, salinity and marine flow velocity, inevitably introduces errors into the above two parameters, and when the measured data outlier caused by non-gaussian noise occurs in the DVL measured data, the accuracy of the estimated result will be greatly reduced, even the divergence occurs. In practical engineering, the corresponding filtering parameters are determined by performing off-line analysis on the collected experimental data of the sea area to be measured. However, when the autonomous underwater vehicle navigates in an unknown sea area, the corresponding measurement noise covariance matrix is often unknown, and if the navigation state is still estimated by using the preset filtering parameters, the obtained state estimation will contain a large amount of errors, which will undoubtedly reduce the task success rate of the autonomous underwater vehicle. In addition, the distribution mode of the Kalman filter with respect to the state noise covariance matrix is single, and self-adaptive adjustment cannot be effectively made according to the working condition of each subsystem.
The interactive multi-model algorithm can adapt to a transformed environment by performing model switching in a preset large number of model sets. Therefore, under variable operating environments, the performance is often superior to that of the traditional Kalman filtering. However, when using an interactive multi-model algorithm, it is necessary to introduce as many models as possible into the system model set to ensure that the real conditions can be accurately described at any time, otherwise the accuracy is reduced. However, as the number of models in the model set increases, competition between the models becomes intense, thereby affecting computational efficiency. Meanwhile, the interactive multi-model has weak resistance to measurement outliers caused by non-Gaussian noise, and the robustness of the interactive multi-model still has a large space for improvement. How to balance navigation accuracy, robustness and calculation efficiency is an important research direction which is still not developed at present, and the long-term operation capability of the underwater robot is restricted.
Disclosure of Invention
It is an object of the present invention to provide a method, device and system for robust filtering for on-line reconstruction of adaptive models that overcomes or at least alleviates at least one of the above-mentioned disadvantages of the prior art.
In order to achieve the above object, the present invention provides an adaptive model on-line reconstruction robust filtering method, which is applied to a combined navigation system including multiple models, and includes:
initializing a model set, a Markov probability transfer matrix and a model probability of each model in the model set;
the model set comprises n preset models;
the Markov probability transition matrix is:
Figure BDA0003616250200000021
p ij representing the probability of a transition from model i to model j;
the sum of the model probabilities of all the models in the model set is 1, and the model probability of each model in the model set is initialized to a preset value;
step two, obtaining the input state quantity and the state covariance matrix of each model according to the following formula:
Figure BDA0003616250200000022
Figure BDA0003616250200000023
wherein the content of the first and second substances,
Figure BDA0003616250200000024
representing the input state quantity of the model j at the current moment k;
Figure BDA0003616250200000025
representing the output state quantity of the model i at the last moment k-1; mu.s ij(k-1/k-1) K model representing the current timeThe weight of the input state quantity of i in the state quantity of model j;
P oj(k-1/k-1) represents the covariance matrix, P, of the model j at the current time k i(k-1/k-1) Representing the state covariance matrix output by the model i at the last moment k-1;
thirdly, filtering and fusing data information acquired by the integrated navigation system by using a robust filter, and outputting state vectors corresponding to the models:
wherein, for each model, a current time k state truth value X is defined k The predicted value of the state of the current time k from the previous time k-1
Figure BDA0003616250200000031
The relationship between them is:
Figure BDA0003616250200000032
where δ X is the prediction error and the corresponding variance is P (k/k-1) The system equation is constructed as follows:
Figure BDA0003616250200000033
wherein, y k Representing the measurement vector, H k Representing a predetermined measurement matrix, I 15x15 Representing a 15-dimensional identity matrix, v k Representing measurement noise;
wherein the kernel function of the robust filter is ρ (e):
Figure BDA0003616250200000034
where e denotes a residual vector, r 0 ，r 1 Is a preset constant;
obtaining state vectors corresponding to the models according to the formulas (3) and (4);
step four, updating the model probability of each model:
wherein, the likelihood function corresponding to the model j is calculated as follows:
Figure BDA0003616250200000035
Figure BDA0003616250200000036
model probability mu for model j according to j(k) Updating:
Figure BDA0003616250200000037
Figure BDA0003616250200000038
wherein p is ij Representing the probability of a transition from model i to model j, n being the total number of models in the set of models, μ i (k-1) is the model probability of the model i at the previous moment k-1;
step five, mixing the covariance matrix of each model, the state vector corresponding to each model and the updated model probability to obtain the state vector X of the carrier (k/k) And a state covariance matrix:
Figure BDA0003616250200000041
Figure BDA0003616250200000042
Figure BDA0003616250200000043
wherein X (k/k) Indicating the current time k carrierIs determined by the state vector of (a),
Figure BDA0003616250200000044
state vector, μ, representing model i at the current time k i(k) Model probability, P, representing model i at the current time k i(k/k) Representing the covariance matrix of the model i at the current time instant k.
Preferably, step three comprises:
order:
Figure BDA0003616250200000045
Figure BDA0003616250200000046
Figure BDA0003616250200000047
Figure BDA0003616250200000048
wherein R is K Representing a preset diagonal matrix; obtaining:
Z k ＝M k X k +ξ k
defining a cost function based on the robust kernel function ρ (e):
τ(X k )＝∑ρ(e i )
wherein e i The ith component of e;
order:
Figure BDA0003616250200000049
Figure BDA0003616250200000051
obtaining:
Figure BDA0003616250200000052
is recorded as:
Ψ＝diag[ψ(e i )]
minimizing the cost function yields:
Figure BDA0003616250200000053
solving each model by using an iterative method to obtain a filtering result corresponding to each model; wherein, the iteration process is as follows:
Figure BDA0003616250200000054
Figure BDA0003616250200000055
where j represents the number of iterations.
Preferably, the method further comprises: adaptively updating models within the set of models, comprising:
order:
ξ k ＝Z k -M k X k
Figure BDA0003616250200000056
φ i ＝D i(k-1) μ i(k-1) +D i(k-2) μ i(k-2) +…+D i(k-l) μ i(k-l)
i＝1,2,…,n
Figure BDA0003616250200000057
wherein, the value of phi is used for representing the variance change level of the measurement information at the current moment, l is the number of preset states nearest to the current state, k-l represents the previous l moments of k at the current moment, and (trace) represents the trace of the matrix; when phi is larger than the preset value phi 0 Adaptive updating is performed.
Preferably, the adaptively updating the models in the model set further comprises:
order:
Figure BDA0003616250200000061
Figure BDA0003616250200000062
wherein λ 0 、η、λ max All are preset values, and lambda represents the change proportion of each model in the model set after updating.
Preferably, the method further comprises: and when the model set is reconstructed, taking the model occupying the largest weight in the original model set as a main model, and keeping the proportional relation among the models in the original model set.
The embodiment of the invention also provides an adaptive model online reconstruction robust filtering device, which is applied to a combined navigation system comprising a plurality of models and comprises the following steps:
the initialization module is used for initializing a model set, a Markov probability transfer matrix and the model probability of each model in the model set;
the model set comprises n preset models;
the Markov probability transition matrix is:
Figure BDA0003616250200000063
p ij representing the transition probability from model i to model j;
the sum of the model probabilities of all the models in the model set is 1, and the model probability of each model in the model set is initialized to a preset value;
an input module, configured to obtain an input state quantity and a state covariance matrix of each model according to the following formula:
Figure BDA0003616250200000064
Figure BDA0003616250200000065
wherein the content of the first and second substances,
Figure BDA0003616250200000066
representing the input state quantity of the model j at the current moment k;
Figure BDA0003616250200000067
representing the output state quantity of the model i at the last moment k-1; mu.s ij(k-1/k-1) Representing the weight of the input state quantity of the model i at the current moment k in the state quantity of the model j;
P oj(k-1/k-1) represents the covariance matrix, P, of the model j at the current time k i(k-1/k-1) Representing the state covariance matrix output by the model i at the last moment k-1;
and the output module is used for filtering and fusing the data information acquired by the integrated navigation system by using a robust filter and outputting the state vectors corresponding to the models:
wherein, for each model, a current time k state truth value X is defined k The predicted value of the state of the current time k from the previous time k-1
Figure BDA0003616250200000071
The relationship between them is:
Figure BDA0003616250200000072
where δ X is the prediction error and the corresponding variance is P (k/k-1) The system equation is constructed as follows:
Figure BDA0003616250200000073
wherein, y k Representing the measurement vector, H k Represents a predetermined measurement matrix, I 15x15 Representing a 15-dimensional identity matrix, v k Representing measurement noise;
wherein the kernel function of the robust filter is ρ (e):
Figure BDA0003616250200000074
where e denotes a residual vector, r 0 ，r 1 Is a preset constant;
obtaining state vectors corresponding to the models according to the formulas (3) and (4);
a model probability updating module for updating the model probabilities of the respective models:
and calculating a likelihood function corresponding to the model j:
Figure BDA0003616250200000075
Figure BDA0003616250200000076
model probability mu for model j according to j(k) Updating:
Figure BDA0003616250200000081
wherein p is ij Representing the probability of a transition from model i to model j, n being the total number of models in the set,μ i (k-1) is the model probability of the model i at the previous moment k-1;
a state determining module, configured to mix the covariance matrix of each model, the state vector corresponding to each model, and the updated model probability to obtain a state vector X of the carrier (k/k) And a state covariance matrix:
Figure BDA0003616250200000082
Figure BDA0003616250200000083
wherein, X (k/k) A state vector representing the k carriers at the current instant,
Figure BDA0003616250200000084
state vector, μ, representing model i at the current time k i(k) Model probability, P, representing model i at the current time k i(k/k) Representing the covariance matrix of model i at the current time instant k.
Preferably, the output module is configured to:
order:
Figure BDA0003616250200000085
Figure BDA0003616250200000086
Figure BDA0003616250200000087
Figure BDA0003616250200000088
wherein R is K Representing a preset diagonal matrix; obtaining:
Z k ＝M k X k +ξ k
defining a cost function based on the robust kernel function ρ (e):
τ(X k )＝∑ρ(e i )
wherein e i The ith component of e;
order:
Figure BDA0003616250200000091
Figure BDA0003616250200000092
obtaining:
Figure BDA0003616250200000093
is recorded as:
Ψ＝diag[ψ(e i )]
minimizing the cost function yields:
Figure BDA0003616250200000094
solving each model by using an iterative method to obtain a filtering result corresponding to each model; wherein, the iteration process is as follows:
Figure BDA0003616250200000095
Figure BDA0003616250200000096
where j represents the number of iterations.
Preferably, the apparatus further comprises: a model update module for adaptively updating models in the model set, comprising:
order:
ξ k ＝Z k -M k X k
Figure BDA0003616250200000097
φ i ＝D i(k-1) μ i(k-1) +D i(k-2) μ i(k-2) +…+D i(k-l) μ i(k-l)
i＝1,2,…,n
Figure BDA0003616250200000101
wherein, the value of phi is used for representing the variance change level of the measurement information at the current moment, l is the number of preset states nearest to the current state, k-l represents the previous l moments of k at the current moment, and (trace) represents the trace of the matrix; when phi is larger than the preset value phi 0 Adaptive updating is performed.
Preferably, the model update module is further configured to:
order:
Figure BDA0003616250200000102
Figure BDA0003616250200000103
wherein λ 0 、η、λ max All are preset values, and lambda represents the change proportion of each model in the model set after updating.
The embodiment of the invention also provides an adaptive model online reconstruction robust filtering system, which comprises the adaptive model online reconstruction robust filtering device in the embodiment and any implementation mode thereof.
Due to the adoption of the technical scheme, the invention has the following advantages:
the robust filter is used for filtering and fusing the combined navigation system, adverse effects caused by measurement outliers can be effectively resisted, the total size of the interactive multi-model preset model set can be effectively reduced, and meanwhile, the robustness and the accuracy of the combined navigation system can be further improved as the main model is not limited when a new model set is constructed.
Drawings
Fig. 1 is a schematic flowchart of a method for reconstructing a robust filter on line by using an adaptive model according to an embodiment of the present invention.
Fig. 2 is a schematic flow chart of an adaptive model online reconstruction robust filtering method according to an example of the present invention.
Fig. 3 is a schematic structural diagram of an adaptive model on-line reconstruction robust filtering apparatus according to an embodiment of the present invention.
Fig. 4 is a schematic structural diagram of an adaptive model on-line reconstruction robust filtering system according to an example of the present invention.
Detailed Description
In the drawings, the same or similar reference numerals are used to denote the same or similar elements or elements having the same or similar functions. Embodiments of the present invention will be described in detail below with reference to the accompanying drawings.
In the description of the present invention, the terms "central," "longitudinal," "lateral," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," and the like refer to orientations or positional relationships that are based on the orientation or positional relationship shown in the drawings, merely for convenience in describing the invention and to simplify the description, and do not indicate or imply that the referenced devices or elements must have a particular orientation, be constructed in a particular orientation, and be operated, and therefore should not be construed as limiting the scope of the invention.
In the present invention, the technical features of the embodiments and implementations may be combined with each other without conflict, and the present invention is not limited to the embodiments or implementations in which the technical features are located.
The present invention will be further described with reference to the accompanying drawings and specific embodiments, it should be noted that the technical solutions and design principles of the present invention are described in detail in the following only by way of an optimized technical solution, but the scope of the present invention is not limited thereto.
The following terms are referred to herein, and their meanings are explained below for ease of understanding. It will be understood by those skilled in the art that the following terms may have other names, but any other names should be considered consistent with the terms set forth herein without departing from their meaning.
The invention provides an on-line reconstruction robust filtering method for a self-adaptive model. As shown in fig. 1, the method includes:
step 10, initializing a model set, a Markov probability transition matrix and a model probability of each model in the model set.
The model set comprises n preset models.
The Markov probability transition matrix is:
Figure BDA0003616250200000111
p ij representing the probability of a transition from model i to model j;
the sum of the model probabilities of all the models in the model set is 1, and the model probability of each model in the model set is initialized to a preset value. The model probability for each model represents the weight of that model in all models.
In one example, the total number of models in the entire model set can be set to three, in R K1 、R K2 、R K3 Respectively representing the corresponding filter parameters of the three models.
The models in the model set are initialized, for example, as follows:
R K ＝diag([0.1,0.1,0.1]) 2
R K representing diagonal matrices, the specific size beingThe setting is based on the actual measured characteristics of the sensor.
Figure BDA0003616250200000121
λ 0 For the preset value, lambda can be set according to engineering experience 0 ＝2。
In one example, the markov probability transition matrix P may be initialized as:
Figure BDA0003616250200000122
in one example, each model corresponds to a model probability of μ i It can be initialized as:
Figure BDA0003616250200000123
setting an initialization matrix mu ij ，μ ij Representing the weight of the input state quantity of each model in the input interaction in the actual system state quantity, introducing an intermediate quantity
Figure BDA0003616250200000124
Figure BDA0003616250200000125
Figure BDA0003616250200000126
Step 20, obtaining the input state quantity and the state covariance matrix of each model according to the following formula:
Figure BDA0003616250200000127
Figure BDA0003616250200000128
wherein the content of the first and second substances,
Figure BDA0003616250200000131
representing the input state quantity of the model j at the current moment k;
Figure BDA0003616250200000132
representing the output state quantity of the model i at the last moment k-1; mu.s ij(k-1/k-1) Representing the weight of the input state quantity of the model i at the current moment k in the state quantity of the model j;
P oj(k-1/k-1) represents the covariance matrix, P, of the model j at the current time k i(k-1/k-1) Representing the state covariance matrix output by model i at the last time k-1.
In one example, the pair
Figure BDA0003616250200000133
And P i(k-1/k-1) The initialization is as follows:
Figure BDA0003616250200000134
Figure BDA0003616250200000135
the medium state quantity respectively corresponds to three-direction speed estimation errors of the carrier, three-direction angle errors, three-direction position errors and random errors of an accelerometer and a gyroscope,
Figure BDA0003616250200000136
is a 15x1 dimensional vector.
Figure BDA0003616250200000137
[100 100 100]*ug[0.02 0.02 0.02]*dph) 2
Wherein (diag. Cndot.) represents a diagonal matrix,
Figure BDA0003616250200000138
re represents the earth radius, re =6378137m, ug = g 10 -6 And g represents the acceleration of gravity,
Figure BDA0003616250200000139
in actual engineering, P i(0/0) The adjustment in parameters should be made according to the actual performance of the sensor.
And step 30, filtering and fusing the data information acquired by the integrated navigation system by using a robust filter, and outputting the state vector corresponding to each model.
Wherein, for each model, a current time k state truth value X is defined k And the predicted value of the previous time k-1 to the time k
Figure BDA00036162502000001310
The relationship between them is:
Figure BDA00036162502000001311
where δ X is the prediction error and the corresponding variance is P (k/k-1) The system equation is constructed as follows:
Figure BDA00036162502000001312
wherein, y k Representing the measurement vector, H k Denotes a measurement matrix, I 15x15 Representing a 15-dimensional identity matrix, v k Representing measurement noise;
wherein the kernel function of the robust filter is ρ (e):
Figure BDA0003616250200000141
where e denotes the residual vector, r 0 ，r 1 Is a preset constant;
and (4) obtaining the state vector corresponding to each model according to the formulas (3) and (4).
In one embodiment, obtaining the state vector corresponding to each model according to equations (3) and (4) includes:
order:
Figure BDA0003616250200000142
Figure BDA0003616250200000143
Figure BDA0003616250200000144
Figure BDA0003616250200000145
wherein R is K Representing a preset diagonal matrix; obtaining:
Z k ＝M k X k +ξ k
defining a cost function based on the robust kernel function ρ (e):
τ(X k )＝∑ρ(e i )
wherein e i The ith component of e;
order:
Figure BDA0003616250200000146
Figure BDA0003616250200000147
obtaining:
Figure BDA0003616250200000151
is recorded as:
Ψ＝diag[ψ(e i )]
minimizing the cost function yields:
Figure BDA0003616250200000152
for each model, solving the above formula by using an iterative method to obtain a filtering result corresponding to each model; wherein, the iteration process is as follows:
Figure BDA0003616250200000153
Figure BDA0003616250200000154
where j represents the number of iterations.
Step 40, updating model probabilities of the models:
and calculating a likelihood function corresponding to the model j:
Figure BDA0003616250200000155
Figure BDA0003616250200000156
model probability mu for model j according to j(k) Updating:
Figure BDA0003616250200000157
wherein p is ij Is represented byThe probability of the conversion from model i to model j, n is the total number of models in the model set, mu i And (k-1) is the model probability of the model i at the last moment k-1.
Step 50, mixing the covariance matrix of each model, the state vector corresponding to each model and the updated model probability to obtain the state vector X of the carrier (k/k) And the state covariance matrix:
Figure BDA0003616250200000161
Figure BDA0003616250200000162
wherein, X (k/k) A state vector representing the current time k carrier,
Figure BDA0003616250200000163
state vector, μ, representing model i at the current time k i(k) Model probability, P, representing model i at the current time k i(k/k) Representing the covariance matrix of the model i at the current time instant k.
Besides the above steps, the method can also comprise the following steps:
and step 60, adaptively updating the models in the model set.
Specifically, let:
ξ k ＝Z k -M k X k
Figure BDA0003616250200000164
φ i ＝D i(k-1) μ i(k-1) +D i(k-2) μ i(k-2) +…+D i(k-l) μ i(k-l)
i＝1,2,…,n
Figure BDA0003616250200000165
wherein, the value of phi is used for representing the variance change level of the measurement information at the current time, l is the number of states nearest to the current state and is a preset value, k-l represents the previous l times of the current time k, and (trace) represents the trace of the matrix; when phi is larger than the preset value phi 0 Adaptive updating is performed.
Wherein the adaptively updating the models in the model set may further include:
order:
Figure BDA0003616250200000166
Figure BDA0003616250200000167
wherein λ 0 、η、λ max And the lambda is a preset value and represents the change proportion of each model in the model set after updating.
When the model set is reconstructed, the model occupying the largest weight in the original model set can be used as a main model, and the weight magnitude relation among the models in the original model set is reserved. It will be readily appreciated that other ways may be used, such as, without limitation, the necessity of preserving the relationship of weight magnitudes between models in the original set of models, and without limitation herein.
In one example of the present invention, as shown in fig. 2, the initialization is incorporated into the input interaction, which includes five steps of input interaction, filter prediction, model probability and parameter update, output interaction, and robust adaptive model on-line reconstruction. The method comprises the following specific steps:
1. inputting interaction: the initial model set is given according to the test environment and the sensor characteristics when the algorithm is started, and the Markov probability transition matrix is determined, wherein the state probability (hereinafter, the model probability) occupied by each model in the whole optimization process is determined. And after initialization is completed, when the algorithm normally runs, completing an input interaction process according to an overall optimization result obtained by interaction output and probability weights corresponding to the models, and obtaining input values of the filters.
The initial model set comprises a plurality of Kalman filter models with different noise measurement levels, and the specific selection and the number of the models can be obtained according to experience. In one example, the number of Kalman filter models included in the initial set of models is three, in R K1 、R K2 、R K3 Respectively representing the corresponding filter parameters of the three models.
Model set parameters are initialized. In this example, the model in the model set is initialized as follows using the strategy of online reconstruction of the adaptive model:
R K ＝diag([0.1,0.1,0.1]) 2
R K representing a diagonal matrix, whose specific values can be set according to the actual measured characteristics of the sensor. The filter parameters of the three Kalman filter models are respectively as follows:
Figure BDA0003616250200000171
λ 0 for the preset value, e.g. λ can be set according to engineering experience 0 ＝2。
The kalman filter model is a model known in the art, and an embodiment of the present invention is not described in detail and is briefly described as follows.
The Kalman filtering model consists of two parts, namely time updating and measurement updating, and the specific form is as follows:
and (3) time updating:
Figure BDA0003616250200000172
S (k/k-1) ＝F·S (k-1/k- 1)·F T +Q k
and (3) measurement updating:
Figure BDA0003616250200000181
Figure BDA0003616250200000182
S (k/k) ＝(I-K k H k )S(k /k-1)
wherein, X (k/k) For the state estimation at the time instant k,
Figure BDA0003616250200000183
representing the one-step predicted state quantity at time k.
H k For the measurement matrix, its form is fixed during the update process.
F represents a state transition matrix, the form of which is fixed during the updating process.
K k Representing the kalman gain.
S (k/k) Representing a predicted covariance matrix at time k whose initial values are closely related to the selected sensor characteristics.
Q k Representing the process noise covariance matrix at time k, whose initial values are closely related to the selected sensor characteristics.
The markov probability transition matrix P is:
Figure BDA0003616250200000184
wherein p is ij Representing the probability of a transition from model i to model j, and n is the total number of models in the model set.
In this example, the total number of models is 3, and the initialized markov probability transition matrix P is:
Figure BDA0003616250200000185
the model probability sum of all models is 1, and the model probability corresponding to each model is mu i ，μ i Representing respective filter correspondence modelsThe probability of occupation in the model set is initialized as:
Figure BDA0003616250200000186
initializing the matrix mu ij ，μ ij Representing the weight of the input state quantity of each model in the input interaction in the actual system state quantity, introducing an intermediate quantity
Figure BDA0003616250200000187
Figure BDA0003616250200000188
Figure BDA0003616250200000191
After initialization is completed, processing the state information of the system, including updating the state quantity and the state covariance matrix of the system:
Figure BDA0003616250200000192
j＝1,2,…,n
Figure BDA0003616250200000193
Figure BDA0003616250200000194
wherein
Figure BDA0003616250200000195
Representing the output state quantity, P, of each model at the last moment i(k-1/k-1) Representing the state covariance matrix of each model output at the last time,
Figure BDA0003616250200000196
represents the input state quantity, P, of the model j at the current moment oj(k-1/k-1) Representing the corresponding covariance matrix.
Initial time, pair
Figure BDA0003616250200000197
And P i(k-1/k-1) The initialization is as follows:
Figure BDA0003616250200000198
Figure BDA0003616250200000199
the medium state quantity respectively corresponds to three-direction speed estimation errors of the carrier, three-direction angle errors, three-direction position errors and random errors of an accelerometer and a gyroscope,
Figure BDA00036162502000001910
is a 15x1 dimensional vector.
Figure BDA00036162502000001911
[100 100 100]*ug[0.02 0.02 0.02]*dph) 2
Wherein (diag. Cndot.) represents a diagonal matrix,
Figure BDA00036162502000001912
re represents the earth radius, re =6378137m, ug = g 10 -6 And g represents the acceleration of gravity,
Figure BDA00036162502000001913
in actual engineering, P i(0/0) The adjustment in parameters should be made in accordance with the actual performance of the sensor.
2. Robust filtering prediction: and filtering and fusing data information acquired by the INS/DVL integrated navigation system by using a robust filter formed by combining an improved Huber kernel function and a traditional Kalman filter, performing multiple iterations on an output result through the robust filter, completing a filtering prediction process, and outputting a filtering result corresponding to each model.
And carrying out filtering prediction according to data information acquired by the INS/DVL integrated navigation system according to the following processes:
using a robust kernel function rho (e), in combination with a Kalman filter to obtain a robust filter,
Figure BDA0003616250200000201
wherein e represents an error term (or called residual vector), r 0 ，r 1 Set r as a default constant 0 ＝1.345，r 1 ＝3。
Define the true value X of the state at time k k And predicting the value
Figure BDA0003616250200000202
The relationship between the two is as follows:
Figure BDA0003616250200000203
where δ X is the prediction error and the corresponding variance is P (k/k-1) 。
The system equation is constructed as follows:
Figure BDA0003616250200000204
wherein, y k Representing the measurement vector, H k Denotes a measurement matrix, I 15x15 Representing a 15-dimensional identity matrix, v k Representing the measurement noise.
Wherein y is derived from the measured values obtained by the sensors k In one example, y k And a residual measurement vector consisting of the measured value obtained by the sensor and a predicted value of the model.
H k For a predetermined measurement matrix, when X k And y k When in the same coordinate system, the corresponding dimension can be set as the identity matrix
Order:
Figure BDA0003616250200000205
Figure BDA0003616250200000206
Figure BDA0003616250200000211
Figure BDA0003616250200000212
comprises the following steps:
Z k ＝M k X k +ξ k defining a cost function based on the robust kernel function ρ (e):
τ(X k )＝∑ρ(e i )
wherein e i Is the ith component of the residual vector e.
Order:
Figure BDA0003616250200000213
Figure BDA0003616250200000214
comprises the following steps:
Figure BDA0003616250200000215
recording:
Ψ＝diag[ψ(e i )]then to minimize the corresponding cost function should:
Figure BDA0003616250200000216
and (3) solving the above formula by using an iteration method, wherein the iteration process is as follows:
Figure BDA0003616250200000217
Figure BDA0003616250200000218
where j represents the number of iterations.
Filtering fusion is carried out according to the formula, the output result is iterated for multiple times through a robust filter, the filtering prediction process is completed, and the filtering results corresponding to the models are output
Figure BDA0003616250200000221
Wherein the iteration is terminated when a preset iteration condition is reached. For example, the preset condition is the number of iterations, and the iteration is terminated when the number is reached.
3. Updating model probability and parameters: and calculating a likelihood function and an innovation vector corresponding to each model according to the output of the last step, updating the model probability, switching the models according to a state probability matrix and a known homogeneous Markov chain, and determining the proportion occupied by each model in the output result.
The likelihood function for model j is:
Figure BDA0003616250200000222
Figure BDA0003616250200000223
probability p of transition according to Markov probability ij And the probability mu of the model at the last moment i (k-1) probability μ for model j j(k) Updating:
Figure BDA0003616250200000224
Figure BDA0003616250200000225
4. and an output interaction step:
mixing the obtained model probability weight, state vector and corresponding covariance matrix to obtain the latest model state quantity output X (k/k) And a state covariance matrix:
Figure BDA0003616250200000226
Figure BDA0003616250200000227
Figure BDA0003616250200000228
5. self-adaptive model set weight construction: the sliding window collects the system state quantity and the state covariance matrix obtained by the output interaction module at the preorder moment, constructs an innovation vector according to the measurement information, carries out weighted accumulation, compares the innovation vector with the innovation variance level corresponding to the current moment, obtains a latest model set according to a self-adaptive model set reconstruction strategy, and outputs a result to the robust filtering prediction module.
The aim of covering the current motion state is achieved by self-adaptive updating of the limited model set:
recording the observation residual:
ξ k ＝Z k -M k X k
by D k To reflect the variance level of the measurement state at time k, let:
Figure BDA0003616250200000231
when the measurement information is changed greatly, the model set should be updated, the updating degree of the model set should be determined according to the lambda value and the phi value, and the phi value is calculated by using a sliding window method:
order:
φ i ＝D i(k-1) μ i(k-1) +D i(k-2) μ i(k-2) +…+D i(k-l) μ i(k-l)
i＝1,2,…,n
Figure BDA0003616250200000232
(trace.) represents the trace of the matrix, where l represents the number of states chosen to be nearest neighbors to the current state.
For λ values, there are:
Figure BDA0003616250200000233
Figure BDA0003616250200000234
wherein phi 0 、λ 0 Eta are preset values, e.g. eta = lambda max ＝10，λ 0 ＝2。
Phi is used for measuring the variance change level of the measurement information at the current moment, and when phi is larger than a preset value phi 0 Adaptive updating is performed.
λ represents the variation ratio of each model in the model set to be constructed, for example: new RK1= λ old RK1.
To prevent too large a rate of change of lambda from occurring max By limiting lambda so that the transitions between model sets can be made sequentially without making too large abrupt changes, e.g. by setting phi 0 ＝λ max ＝η＝10。
In one example, when reconstructing the model set, the model occupying the largest weight in the original model set is used as the main model, and the updating process should follow the size relationship between the models in the original model set.
And (5) repeating the steps 1-5 until the navigation system finishes running.
An embodiment of the present invention further provides an adaptive model online reconstruction robust filtering apparatus, which is applied to a combined navigation system including multiple models, and as shown in fig. 3, the apparatus includes:
an initialization module 31, configured to initialize a model set, a markov probability transition matrix, and a model probability of each model in the model set;
the model set comprises n preset models;
the Markov probability transition matrix is:
Figure BDA0003616250200000241
p ij representing the probability of a transition from model i to model j;
the sum of the model probabilities of all the models in the model set is 1, and the model probability of each model in the model set is initialized to a preset value;
an input module 32, configured to obtain an input state quantity and a state covariance matrix of each model according to the following formula:
Figure BDA0003616250200000242
Figure BDA0003616250200000243
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003616250200000244
representing the input state quantity of the model j at the current moment k;
Figure BDA0003616250200000245
representing the output state quantity of the model i at the last moment k-1; mu.s ij(k-1/k-1) Representing the weight of the input state quantity of the model i at the current moment k in the state quantity of the model j;
P oj(k-1/k-1) represents the covariance matrix, P, of the model j at the current time k i(k-1/k-1) Representing a state covariance matrix output by the model i at the last moment k-1;
an output module 33, configured to perform filtering fusion on the data information acquired by the integrated navigation system by using a robust filter, and output a state vector corresponding to each model:
wherein, for each model, a current time k state truth value X is defined k The predicted value of the state of the current time k corresponding to the previous time k-1
Figure BDA0003616250200000251
The relationship between them is:
Figure BDA0003616250200000252
where δ X is the prediction error and the corresponding variance is P (k/k-1) The system equation is constructed as follows:
Figure BDA0003616250200000253
wherein, y k Representing the measurement vector, H k Representing a predetermined measurement matrix, I 15x15 Unit matrix, v, representing 15 dimensions k Representing measurement noise;
wherein the kernel function of the robust filter is ρ (e):
Figure BDA0003616250200000254
where e denotes a residual vector, r 0 ，r 1 Is a preset constant;
obtaining state vectors corresponding to the models according to the formulas (3) and (4);
a model probability updating module 34, configured to update the model probabilities of the respective models:
and calculating a likelihood function corresponding to the model j:
Figure BDA0003616250200000255
Figure BDA0003616250200000256
model probability mu for model j according to j(k) Updating:
Figure BDA0003616250200000257
Figure BDA0003616250200000258
wherein p is ij Representing the probability of a transition from model i to model j, n being the total number of models in the set of models, μ i (k-1) is the model probability of the model i at the previous moment k-1;
a state determining module 35, configured to mix the covariance matrix of each model, the state vector corresponding to each model, and the updated model probability to obtain a state vector X of the carrier (k/k) And a state covariance matrix:
Figure BDA0003616250200000261
Figure BDA0003616250200000262
wherein, X (k/k) A state vector representing the k carriers at the current instant,
Figure BDA0003616250200000263
state vector, μ, representing model i at the current time k i(k) Model probability, P, representing model i at the current time k i(k/k) Representing the covariance matrix of the model i at the current time instant k.
Preferably, the output module 33 is configured to:
order:
Figure BDA0003616250200000264
Figure BDA0003616250200000265
Figure BDA0003616250200000266
Figure BDA0003616250200000267
wherein R is K Representing a preset diagonal matrix; obtaining:
Z k ＝M k X k +ξ k
defining a cost function based on the robust kernel function ρ (e):
τ(X k )＝∑ρ(e i )
wherein e i The ith component of e;
order:
Figure BDA0003616250200000268
Figure BDA0003616250200000269
obtaining:
Figure BDA0003616250200000271
is recorded as:
Ψ＝diag[ψ(e i )]
minimizing the cost function yields:
Figure BDA0003616250200000272
solving each model by using an iterative method to obtain a filtering result corresponding to each model; wherein, the iteration process is as follows:
Figure BDA0003616250200000273
Figure BDA0003616250200000274
where j represents the number of iterations.
In one embodiment, the apparatus further comprises: a model updating module 36 for adaptively updating the models in the model set, including:
order:
ξ k ＝Z k -M k X k
Figure BDA0003616250200000275
φ i ＝D i(k-1) μ i(k-1) +D i(k-2) μ i(k-2) +…+D i(k-l) μ i(k-l)
i＝1,2,…,n
Figure BDA0003616250200000276
wherein, the value of phi is used for representing the variance change level of the measurement information at the current moment, l is the number of preset states nearest to the current state, k-l represents the previous l moments of k at the current moment, and (trace) represents the trace of the matrix; when phi is larger than the preset value phi 0 Adaptive updating is performed.
Preferably, the model updating module 36 is further configured to:
order:
Figure BDA0003616250200000281
Figure BDA0003616250200000282
wherein λ 0 、η、λ max All are preset values, and lambda represents the change proportion of each model in the model set after updating.
The embodiment of the invention also provides an adaptive model online reconstruction robust filtering system which comprises the adaptive model online reconstruction robust filtering equipment in the embodiment and any implementation mode.
In an example, as shown in fig. 4, an adaptive model online reconstruction robust filtering system provided by an embodiment of the present invention includes an INS (inertial navigation system), a DVL (doppler velocimeter), a sensor data acquisition device, a data storage device, and a fusion pose resolving device. The inertial navigation system provides accelerometer information and angular velocity information for the autonomous underwater robot; the Doppler log provides high-precision speed information for the autonomous underwater robot; the sensor data acquisition device acquires data information output by the two sensors; the data storage device stores the data information acquired by the data acquisition unit; and the fusion pose resolving device receives the data information acquired by the two sensors, processes the data information and obtains high-precision fusion pose information. The sensor data acquisition device, the data storage device and the fusion pose resolving device may be different hardware devices independent of each other, or may be integrated in the same hardware device, which is not limited herein. The fusion pose resolving device is used for executing the adaptive model online reconstruction robust filtering method provided by the embodiment and any implementation mode of the embodiment.
In the invention, the robust filter is used for filtering and fusing the combined navigation system, so that adverse effects caused by measurement outliers can be effectively resisted, the total size of the interactive multi-model preset model set can be effectively reduced, and meanwhile, the robustness and the accuracy of the combined navigation system can be further improved because the main model is not limited when a new model set is constructed.
Finally, it should be pointed out that: the above examples are only for illustrating the technical solutions of the present invention, and are not limited thereto. Those of ordinary skill in the art will understand that: modifications can be made to the technical solutions described in the foregoing embodiments, or some technical features may be equivalently replaced; such modifications or substitutions do not depart from the spirit and scope of the corresponding technical solutions of the embodiments of the present invention.

Claims (10)

1. An adaptive model online reconstruction robust filtering method is applied to a combined navigation system comprising a plurality of models, and is characterized by comprising the following steps:
initializing a model set, a Markov probability transition matrix and a model probability of each model in the model set;
the model set comprises n preset models;
the Markov probability transition matrix is:
Figure FDA0003983095390000011
p ij representing the probability of a transition from model i to model j;
the sum of the model probabilities of all the models in the model set is 1, and the model probability of each model in the model set is initialized to a preset value;
step two, obtaining the input state quantity and the state covariance matrix of each model according to the following formula:
Figure FDA0003983095390000012
j＝1，2，…，n
Figure FDA0003983095390000013
Figure FDA0003983095390000014
wherein the content of the first and second substances,
Figure FDA0003983095390000015
representing the input state quantity of the model j at the current moment k;
Figure FDA0003983095390000016
representing the output state quantity of the model i at the last moment k-1; mu.s ij(k-1/k-1) Representing the weight of the input state quantity of the model i at the current moment k in the state quantity of the model j;
P oj(k-1/k-1) represents the covariance matrix, P, of the model j at the current time k i(k-1/k-1) Representing the state covariance matrix output by the model i at the last moment k-1;
thirdly, filtering and fusing data information acquired by the integrated navigation system by using a robust filter, and outputting state vectors corresponding to the models:
wherein, for each model, a current time k state truth value X is defined k The predicted value of the state of the current time k corresponding to the previous time k-1
Figure FDA0003983095390000017
The relationship between them is:
Figure FDA0003983095390000021
where δ X is the prediction error and the corresponding variance is P (k/k-1) The system equation is constructed as follows:
Figure FDA0003983095390000022
wherein, y k Representing the measurement vector, H k Representing a predetermined measurement matrix, I 15x15 Representing a 15-dimensional identity matrix, v k Representing measurement noise;
wherein the kernel function of the robust filter is ρ (e):
Figure FDA0003983095390000023
where e denotes the residual vector, r 0 ，r 1 Is a preset constant;
obtaining state vectors corresponding to the models according to the formulas (3) and (4);
step four, updating the model probability of each model:
and calculating a likelihood function corresponding to the model j:
Figure FDA0003983095390000024
Figure FDA0003983095390000025
where m represents the dimension of the observed quantity, R k Representing a preset diagonal matrix;
model probability mu for model j according to j(k) Updating:
Figure FDA0003983095390000026
Figure FDA0003983095390000027
wherein p is ij Representing the probability of a transition from model i to model j, n being the total number of models in the set of models, μ i (k-1) is the model probability of the model i at the previous moment k-1;
step five, mixing the covariance matrix of each model, the state vector corresponding to each model and the updated model probability to obtain the state vector X of the carrier (k/k) And a state covariance matrix:
Figure FDA0003983095390000031
Figure FDA0003983095390000032
Figure FDA0003983095390000033
wherein, X (k/k) A state vector representing the k carriers at the current instant,
Figure FDA0003983095390000034
state vector, μ, representing model i at the current time k i(k) Model probability, P, representing model i at the current time k i(k/k) Representing the covariance matrix of the model i at the current time instant k.
2. The adaptive model on-line reconstruction robust filtering method according to claim 1, wherein the third step comprises:
order:
Figure FDA0003983095390000035
Figure FDA0003983095390000036
Figure FDA0003983095390000037
Figure FDA0003983095390000038
obtaining:
Z k ＝M k X k +ξ k
defining a cost function based on the robust kernel function ρ (e):
τ(X k )＝∑ρ(e i )
wherein e i The ith component of e;
order:
Figure FDA0003983095390000039
Figure FDA00039830953900000310
obtaining:
Figure FDA0003983095390000041
is recorded as:
Ψ＝diag[ψ(e i )]
minimizing the cost function yields:
Figure FDA0003983095390000042
solving each model by using an iterative method to obtain a filtering result corresponding to each model; wherein the iterative process is as follows:
Figure FDA0003983095390000043
Figure FDA0003983095390000044
where j represents the number of iterations.
3. The adaptive model on-line reconstruction robust filtering method according to claim 2, further comprising: adaptively updating models within the set of models, comprising:
order:
ξ k ＝Z k -M k X k
Figure FDA0003983095390000045
φ i ＝D i(k-1) μ i(k-1) +D i(k-2) μ i(k-2) +…+D i(k-l) μ i(k-l)
i＝1，2，…，n
Figure FDA0003983095390000046
wherein, the value of phi is used for representing the variance change level of the measurement information at the current moment, l is the number of preset states which are nearest to the current state, k-l represents the first l moments of k at the current moment, and (trace) represents the trace of the matrix; when phi is larger than the preset value phi 0 Adaptive updating is performed.
4. The adaptive model online reconstruction robust filtering method as claimed in claim 3, wherein the adaptively updating the models in the model set further comprises:
order:
Figure FDA0003983095390000051
Figure FDA0003983095390000052
wherein λ 0 、η、λ max All are preset values, and lambda represents the change proportion of each model in the model set after updating.
5. The adaptive model on-line reconstruction robust filtering method according to claim 3 or 4, further comprising: and when the model set is reconstructed, taking the model occupying the maximum weight in the original model set as a main model, and keeping the proportional relation among the models in the original model set.
6. An adaptive model online reconstruction robust filtering device applied to a combined navigation system comprising a plurality of models is characterized by comprising:
the initialization module is used for initializing a model set, a Markov probability transfer matrix and the model probability of each model in the model set;
the model set comprises n preset models;
the Markov probability transition matrix is:
Figure FDA0003983095390000053
p ij representing the transition probability from model i to model j;
the sum of the model probabilities of all the models in the model set is 1, and the model probability of each model in the model set is initialized to a preset value;
an input module, configured to obtain an input state quantity and a state covariance matrix of each model according to the following formula:
Figure FDA0003983095390000054
j＝1，2，…，n
Figure FDA0003983095390000055
Figure FDA0003983095390000056
wherein the content of the first and second substances,
Figure FDA0003983095390000061
representing the input state quantity of the model j at the current moment k;
Figure FDA0003983095390000062
representing the output state quantity of the model i at the last moment k-1; mu.s ij(k-1/k-1) Representing the weight of the input state quantity of the model i at the current moment k in the state quantity of the model j;
P oj(k-1/k-1) represents the covariance matrix, P, of the model j at the current time k i(k-1/k-1) Representing a state covariance matrix output by the model i at the last moment k-1;
and the output module is used for filtering and fusing the data information acquired by the integrated navigation system by using a robust filter and outputting the state vectors corresponding to the models:
wherein, for each model, a current time k state truth value X is defined k The predicted value of the state of the current time k corresponding to the previous time k-1
Figure FDA0003983095390000063
The relationship between them is:
Figure FDA0003983095390000064
where δ X is the prediction error and the corresponding variance is P (k/k-1) The system equation is constructed as follows:
Figure FDA0003983095390000065
wherein, y k Representing the measurement vector, H k Represents a predetermined measurement matrix, I 15x15 Representing a 15-dimensional identity matrix, v k Representing measurement noise;
wherein the kernel function of the robust filter is ρ (e):
Figure FDA0003983095390000066
where e denotes the residual vector, r 0 ，r 1 Is a preset constant;
obtaining state vectors corresponding to the models according to the formulas (3) and (4);
a model probability updating module for updating the model probabilities of the respective models:
and calculating a likelihood function corresponding to the model j:
Figure FDA0003983095390000067
Figure FDA0003983095390000068
where m represents the dimension of the observed quantity, R k Representing a preset diagonal matrix;
model probability mu for model j according to j(k) Updating:
Figure FDA0003983095390000071
Figure FDA0003983095390000072
wherein p is ij Representing the probability of a transition from model i to model j, n being the total number of models in the set of models, μ i (k-1) is the model probability of the model i at the previous moment k-1;
a state determining module, configured to mix the covariance matrix of each model, the state vector corresponding to each model, and the updated model probability to obtain a state vector X of the carrier (k/k) And the state covariance matrix:
Figure FDA0003983095390000073
Figure FDA0003983095390000074
Figure FDA0003983095390000075
wherein, X (k/k) A state vector representing the k carriers at the current instant,
Figure FDA0003983095390000076
state vector, μ, representing model i at the current time k i(k) Model probability, P, representing model i at the current time k i(k/k) Representing the covariance matrix of the model i at the current time instant k.
7. The adaptive model in-line reconstruction robust filtering apparatus according to claim 6, wherein the output module is configured to:
order:
Figure FDA0003983095390000077
Figure FDA0003983095390000078
Figure FDA0003983095390000079
Figure FDA00039830953900000710
obtaining:
Z k ＝M k X k +ξ k
defining a cost function based on the robust kernel function ρ (e):
τ(X k )＝∑ρ(e i )
wherein e i The ith component of e;
order:
Figure FDA0003983095390000081
Figure FDA0003983095390000082
obtaining:
Figure FDA0003983095390000083
is recorded as:
Ψ＝diag[ψ(e i )]
minimizing the cost function yields:
Figure FDA0003983095390000084
solving each model by using an iterative method to obtain a filtering result corresponding to each model; wherein the iterative process is as follows:
Figure FDA0003983095390000085
Figure FDA0003983095390000086
where j represents the number of iterations.
8. The adaptive model on-line reconstruction robust filtering apparatus according to claim 7, further comprising: a model update module for adaptively updating models in the model set, comprising:
order:
ξ k ＝Z k -M k X k
Figure FDA0003983095390000091
φ i ＝D i(k-1) μ i(k-1) +D i(k-2) μ i(k-2) +…+D i(k-l) μ i(k-l)
i＝1，2，…，n
Figure FDA0003983095390000092
wherein, the value of phi is used for representing the variance change level of the measurement information at the current moment, l is the number of preset states nearest to the current state, k-l represents the previous l moments of k at the current moment, and (trace) represents the trace of the matrix; when phi is larger than the preset value phi 0 Adaptive updating is performed.
9. The adaptive model online reconstruction robust filtering apparatus of claim 8, wherein the model update module is further configured to:
order:
Figure FDA0003983095390000093
Figure FDA0003983095390000094
wherein λ 0 、η、λ max All are preset values, and lambda represents the change proportion of each model in the model set after updating.
10. An adaptive model on-line reconstruction robust filtering system comprising the adaptive model on-line reconstruction robust filtering apparatus according to any one of claims 6 to 9.
CN202210448169.8A 2022-04-24 2022-04-24 Adaptive model on-line reconstruction robust filtering method, device and system Active CN114741659B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210448169.8A CN114741659B (en) 2022-04-24 2022-04-24 Adaptive model on-line reconstruction robust filtering method, device and system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210448169.8A CN114741659B (en) 2022-04-24 2022-04-24 Adaptive model on-line reconstruction robust filtering method, device and system

Publications (2)

Publication Number Publication Date
CN114741659A CN114741659A (en) 2022-07-12
CN114741659B true CN114741659B (en) 2023-01-31

Family

ID=82284305

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210448169.8A Active CN114741659B (en) 2022-04-24 2022-04-24 Adaptive model on-line reconstruction robust filtering method, device and system

Country Status (1)

Country Link
CN (1) CN114741659B (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117572408B (en) * 2024-01-17 2024-04-16 中国人民解放军海军航空大学 Radar high-speed high-maneuvering target tracking method under large measurement error

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8600660B2 (en) * 2006-09-29 2013-12-03 Honeywell International Inc. Multipath modeling for deep integration
CN106352876B (en) * 2016-07-25 2018-05-25 北京航空航天大学 A kind of airborne distribution POS Transfer Alignments based on H ∞ and CKF mixed filterings
CN110375731A (en) * 2019-07-01 2019-10-25 东南大学 A kind of mixing interacting multiple model filters method
CN113762069B (en) * 2021-07-23 2022-12-09 西安交通大学 Long sequence robust enhancement rapid trend filtering method under any noise
CN113932815B (en) * 2021-10-19 2023-07-18 北京京航计算通讯研究所 Robustness optimization Kalman filtering relative navigation method, device, equipment and storage medium

Also Published As

Publication number Publication date
CN114741659A (en) 2022-07-12

Similar Documents

Publication Publication Date Title
CN111156987B (en) Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF
CN111983927A (en) Novel maximum entropy ellipsoid collective filtering method
CN114741659B (en) Adaptive model on-line reconstruction robust filtering method, device and system
CN115451952B (en) Multi-system integrated navigation method and device for fault detection and robust adaptive filtering
Bai et al. A Robust Generalized $ t $ Distribution-Based Kalman Filter
CN116150934A (en) Ship maneuvering Gaussian process regression online non-parameter identification modeling method
CN113340324B (en) Visual inertia self-calibration method based on depth certainty strategy gradient
Cohen et al. A-KIT: Adaptive Kalman-informed transformer
CN113156473A (en) Self-adaptive discrimination method for satellite signal environment of information fusion positioning system
CN116772867A (en) Multi-AUV self-adaptive co-location method and system based on node optimization of factor graph
CN114061592B (en) Adaptive robust AUV navigation method based on multiple models
CN114858166B (en) IMU attitude resolving method based on maximum correlation entropy Kalman filter
CN116734860A (en) Multi-AUV self-adaptive cooperative positioning method and system based on factor graph
CN116608859A (en) Navigation method, storage medium and device of self-adaptive unscented Kalman filtering based on threshold processing
CN114018250B (en) Inertial navigation method, electronic device, storage medium and computer program product
CN115469314A (en) Uniform circular array steady underwater target azimuth tracking method and system
CN114323007A (en) Carrier motion state estimation method and device
CN114460327A (en) Array control method and system for improving anti-interference capability of MEMS sensor
CN110514209B (en) Interactive multi-model combined navigation method
CN114001759A (en) Array type MEMS sensor control method and system
CN113792412A (en) Spacecraft attitude determination method based on central error entropy criterion volume Kalman filtering
CN111426322A (en) Adaptive target tracking filtering method and system for simultaneously estimating state and input
CN111578936A (en) IMM-UKF-based inertial/ultra-short baseline multi-parameter calibration method
CN115451945A (en) Combined navigation post-processing method and system based on Kalman filtering combined graph optimization
CN117268381A (en) Spacecraft state judging method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant