CN108490472B - Unmanned ship integrated navigation method based on fuzzy adaptive filtering - Google Patents

Unmanned ship integrated navigation method based on fuzzy adaptive filtering Download PDF

Info

Publication number
CN108490472B
CN108490472B CN201810082656.0A CN201810082656A CN108490472B CN 108490472 B CN108490472 B CN 108490472B CN 201810082656 A CN201810082656 A CN 201810082656A CN 108490472 B CN108490472 B CN 108490472B
Authority
CN
China
Prior art keywords
noise
input
filtering
matrix
value
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
CN201810082656.0A
Other languages
Chinese (zh)
Other versions
CN108490472A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201810082656.0A priority Critical patent/CN108490472B/en
Publication of CN108490472A publication Critical patent/CN108490472A/en
Application granted granted Critical
Publication of CN108490472B publication Critical patent/CN108490472B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement

Abstract

The invention discloses an unmanned ship integrated navigation method based on fuzzy self-adaptive filtering, which consists of a strapdown inertial navigation system and a global positioning system. The unmanned ship has a small ship body and is greatly influenced by the environment when the unmanned ship executes tasks, and the invention aims to solve the problem of conventional Kalman filtering divergence caused by the fact that the statistical characteristics of measurement noise are different along with the actual external environment and improve the filtering precision of a strapdown inertial navigation system/global satellite navigation system. The method comprises the steps of monitoring the covariance consistency degree of theoretical residual errors and actual residual errors, continuously adjusting the gain coefficient of a filter by using a fuzzy inference system, carrying out online self-adaptive adjustment on a Kalman filter, and finally realizing optimal estimation. The self-adaptive filtering method designed by the invention can accurately estimate the statistical characteristics of the real-time measurement noise of the system, so that the unmanned ship combined navigation system outputs more accurate position and speed information.

Description

Unmanned ship integrated navigation method based on fuzzy adaptive filtering
Technical Field
The invention relates to the technical field of navigation systems, in particular to an unmanned ship combined navigation method based on fuzzy adaptive filtering.
Background
The Unmanned Ship (USV) has the advantages of small volume, good stealth, intelligence, no casualties and the like, can flexibly fight in the military aspect, is mobile to deploy and convenient to use, can independently and autonomously execute tasks in dangerous areas or areas which are not suitable for dispatching manned ships, expands the maritime fighting range and has good cost-effectiveness ratio. As a developing middle and large country with wide coastline and frequent marine disputes, the unmanned boat technology research needs to be strengthened to protect the marine interests of China. And the design of a high-performance navigation system for the navigation system has important practical significance. At present, main requirements for a navigation system of an unmanned ship are small volume, high precision and high reliability, and the unmanned ship navigation system can adapt to different sea conditions.
In a traditional integrated navigation system, a Kalman filtering technology is widely applied. When the mathematical model of the integrated navigation system is accurately known and the calculation error is ignored, the state of the system is estimated by using the conventional Kalman filtering, and an accurate estimation value of the state can be obtained. If the system mathematical model is not accurate or the noise statistical characteristics are changed, the precision of the conventional Kalman filtering is greatly reduced and even diverged. Establishing an accurate mathematical model of the integrated navigation system requires a great deal of experimentation, especially establishing accurate statistical properties of system noise and measurement noise. In order to solve the problem, an adaptive filtering technology is generally adopted, and information brought by observation data is utilized to continuously estimate and correct model parameters and noise statistical characteristics on line so as to improve filtering precision and obtain an optimal estimated value of an object state while filtering.
In recent years, adaptive filtering has been intensively studied by many scholars. The literature, "research on an anti-outlier adaptive kalman filtering method, the chinese technical literature, 2003" proposes an anti-outlier adaptive filtering method, which determines whether an outlier appears by monitoring information, and achieves the purpose of eliminating the outlier influence by adaptively adjusting a gain matrix when the outlier appears. But it only solves the situation of filter divergence caused by outliers, but does not have the ability to filter divergence caused by other reasons. The document "A robust and self-testing Kalman filter for autonomous distributed filtration, Degreee of vector of Washington University, 2001" proposes a multi-model and information-based adaptive filtering method, because the method has matrix inversion calculation at multiple positions, the real-time and the stability are difficult to meet the requirements. In addition, this method requires that the system be observable and that it be only suitable for white noise. In the literature, "application research of neural network assisted kalman filtering technology in integrated navigation systems, the chinese technical literature on inertia, 2003", it is pointed out that the integration of artificial intelligence technology and filtering technology is a development trend of filtering technology. The neural network is used as a field in artificial intelligence technology, and has the main advantage that the neural network has no special requirements on a model of a system, and the trained neural network can be used for approximately replacing the original system as long as enough prior data for training exist. But the acquisition of training samples has been a bottleneck in neural network applications.
For the unmanned ship combined navigation system, as the unmanned ship is small and is more easily influenced by complex sea conditions, the statistical characteristic of the noise measured by the system changes along with the actual working environment, and the initial prior value cannot represent the noise condition in the actual working process. Through a large number of repeated tests on the strapdown inertial navigation system, the statistical characteristic of the system noise during the test can be obtained, but the statistical characteristic of the measured noise during the actual work is unknown, so that an adaptive filtering method for estimating the statistical characteristic of the measured noise on line is needed.
Disclosure of Invention
The invention aims to solve the problems and provides an unmanned ship combined navigation method based on fuzzy adaptive filtering. The invention adjusts the measurement noise matrix on line in real time by a designed Fuzzy Inference System (FIS) according to the ratio and the difference of the actual variance and the theoretical variance of the measurement innovation obtained in real time. This eliminates the need for the filtering algorithm to obtain a priori knowledge of the accurate measurement noise matrix, and also allows an accurate estimate of the time-varying measurement noise. The measured noise variance matrix of the system is gradually adjusted to approach the true value in a recursion mode, the approach process can be adjusted to achieve the optimal balance between rapidity and stability by adjusting parameters, and the difference value and the ratio value of the actual variance and the theoretical variance of the measured innovation are used as the input of a fuzzy inference system, so that the system has higher universality.
The invention relates to an unmanned ship combined navigation method based on fuzzy adaptive filtering, which specifically comprises the following steps:
the method comprises the following steps: establishing a system state equation and a measurement equation:
in an SINS/GPS combined navigation mode, an SINS is used as a main navigation system to establish a state equation of the system, longitude and latitude errors, speed errors and misalignment angles are selected as state variables, and a difference value of speed and position provided by the GPS and the SINS is used as a measurement variable to establish a measurement equation of the combined navigation system;
the state equation is as follows: xk＝Φk,k-1Xk-1+Γk-1Wk-1
The measurement equation is as follows: zk＝HkXk+Vk
Wherein, XkIs a system state vector, ZkFor measuring the vector, HkFor measuring the matrix,. phik,k-1Being a state transition matrix, Γk-1Is a system noise matrix, Wk-1Is a system noise vector, VkTo measure the noise vector; wk-1And VkIs an uncorrelated white gaussian noise sequence, and the mean and variance are respectively: e { Wk}＝0,E{WkWj T}＝Qkδkj,E{Vk}＝0,E{VkVj T}＝Rkδkj，cov{WiVj0; in the formula, QkIs a system noise variance matrix; rkTo measure the noise variance matrix, δkjIs a delta function;
step two: according to a conventional Kalman filtering algorithm, establishing a fuzzy self-adaptive filtering algorithm:
firstly, updating the state one-step predicted value and the mean square error thereof, and utilizing k moment measurement information ZkAnd one-step prediction
Figure GDA0003215722720000031
Calculating an innovation sequence rkSolving a gain matrix and a filtering equation;
and (3) one-step prediction:
Figure GDA0003215722720000032
predicting the mean square error:
Figure GDA0003215722720000033
gain matrix:
Figure GDA0003215722720000034
the innovation sequence is as follows:
Figure GDA0003215722720000035
the filter equation:
Figure GDA0003215722720000036
filtering mean square error: pk＝(I-KkHk)Pk/k-1
In the description of the algorithm described above,
Figure GDA0003215722720000037
for the state vector X in the filtering processkThe estimated amount of (a) is,
Figure GDA0003215722720000038
for one-step prediction of state, KkFor filter gain, PkBeing a covariance matrix of the filtering errors, Pk/k-1For prediction error covariance matrix, rkIs an innovation sequence, and I is an identity matrix;
step three: determining input and output parameters of the fuzzy inference system:
the theoretical value defining the variance of the residual is
Figure GDA0003215722720000039
And the actual value of the residual variance is
Figure GDA00032157227200000310
Two Fuzzy Inference Systems (FIS) were designed, both in single input single output mode. Ratio ROR of theoretical value to actual valuekAs a fuzzy inferenceThe input and output of the physical system 1 are alphak(ii) a Difference DOR between theoretical value and actual valuekB is used as the input of the fuzzy inference system 2;
theoretical value of residual variance:
Figure GDA00032157227200000311
actual value of residual variance:
Figure GDA00032157227200000312
ratio of theoretical value to actual value:
Figure GDA00032157227200000313
difference between theoretical value and actual value:
Figure GDA00032157227200000314
wherein the content of the first and second substances,
Figure GDA00032157227200000315
to average the latest N residual vector variances (N is empirically chosen and generally between 10 and 30, and mainly plays a smoothing role), i0＝k-N+1；riNamely a residual sequence; tr (-) denotes tracing the matrix;
step four: designing a fuzzy inference system:
the input of the fuzzy inference system 1 is the ROR found in the previous stepkOutput is alphakFuzzification is carried out on input and output, membership functions of the input and output are both triangular membership functions, a gravity center method is adopted for defuzzification, and the fuzzy control rule is as follows:
If RORk∈D(Decrease),thenαk∈D(Decrease)
If RORk∈M(Maintain),thenαk∈M(Maintain)
If RORk∈I(Increase),thenαk∈I(Increase)
the input of the fuzzy inference system 2 is the previous stepDetermined DORkAnd b is output, the input and the output are fuzzified, membership functions of the input and the output are both triangular membership functions, the fuzzification adopts a gravity center method, and the fuzzy control rule is as follows:
If DORk∈N(Negtive),thenb∈I(Increase)
If DORk∈Z(Zero),thenb∈M(Maintain)
If DORk∈P(Positive),thenb∈I(Increase)
step five: updating the measurement noise estimation value:
from alpha found in the previous stepkAnd b, updating the measurement noise estimation value, wherein the specific expression is as follows:
measuring a noise estimation value:
Figure GDA0003215722720000041
wherein the content of the first and second substances,
Figure GDA0003215722720000042
representing the measured noise estimate, alpha, of the k-th stepkRepresenting measurement noise
Figure GDA0003215722720000043
B is an exponential adjustment coefficient, representing the adjustment to alphakThe degree of magnification of. If b is>1, represents an amplification of αkTo pair
Figure GDA0003215722720000044
The adjustment function of (2) so that when the noise variation is measured,
Figure GDA0003215722720000045
the true value can be quickly approximated in a small number of steps. If b is<1, represents reduction of αkTo pair
Figure GDA0003215722720000046
The regulating action of (1). b is too large, which may result in
Figure GDA0003215722720000047
Carrying out small amplitude oscillation by taking the real measured noise value as the center; b too small value will result in
Figure GDA0003215722720000048
The transition time to adjust to the true value is slightly longer.
Step six: the filtering process is repeatedly performed:
and continuously repeating the second, third, fourth and fifth steps, carrying out online estimation on the measured noise by using a self-adaptive filtering method, and correcting the navigation information output by the strapdown inertial navigation system in real time until the SINS/GPS integrated navigation process is finished.
The invention has the advantages that:
(1) the unmanned ship combined navigation method based on the fuzzy self-adaptive filtering can effectively solve the problems that the unmanned ship is influenced by the external environment, the navigation precision is reduced and even the filtering is dispersed due to the real-time change of the measured noise, and the like, and overcomes the defects of the traditional algorithm;
(2) the unmanned ship combined navigation method based on the fuzzy adaptive filtering provided by the invention has the advantages that the covariance consistency degree of the theoretical residual and the actual residual is monitored, the fuzzy inference system is used for continuously adjusting the measurement noise and the gain coefficient of the filter, the on-line adaptive adjustment is carried out on the Kalman filter, the SINS/GPS combined navigation precision is ensured, and the method is simple and easy to implement and has universality.
Drawings
FIG. 1: the invention provides a structural block diagram of a navigation system of an unmanned ship self-adaptive filtering method.
FIG. 2: the invention provides a flow chart of an unmanned ship self-adaptive filtering method.
FIG. 3: the input and output membership function of the fuzzy inference system 1 is disclosed.
FIG. 4: the input and output membership function of the fuzzy inference system 2 is disclosed.
FIG. 5: the adaptive filtering and the conventional Kalman filtering curve chart in the invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples.
The fuzzy adaptive filtering method of the unmanned ship integrated navigation system provided by the invention has the structural block diagram and the algorithm flow diagram which are respectively shown in fig. 1 and fig. 2, and specifically comprises the following steps:
the method comprises the following steps: establishing a system state equation and a measurement equation:
in an SINS/GPS combined navigation mode, an SINS is used as a main navigation system to establish a state equation of the system, longitude and latitude errors, speed errors and misalignment angles are selected as state variables, and a difference value of speed and position provided by the GPS and the SINS is used as a measurement variable to establish a measurement equation of the combined navigation system;
firstly, establishing a state equation and a measurement equation of a continuous system:
the state equation is as follows:
Figure GDA0003215722720000051
the measurement equation is as follows: z (t) h (t) x (t) + v (t)
Wherein, the state variable is:
Figure GDA0003215722720000052
state transition array:
Figure GDA0003215722720000053
Figure GDA0003215722720000054
system noise array:
Figure GDA0003215722720000061
attitude angle matrix:
Figure GDA0003215722720000062
measurement coefficient array:
Figure GDA0003215722720000063
in the formula (I), the compound is shown in the specification,
Figure GDA0003215722720000064
δ λ represents a latitude and longitude position error; delta VE、δVNRepresenting east and north speed errors; vE、VNRepresenting east and north speeds;
Figure GDA0003215722720000065
representing east, north and vertical misalignment angles; f. ofe、fn、fuRepresenting the acceleration of the accelerometer in the east, north and vertical directions; w is aieRepresenting the rotational angular velocity of the earth; r represents the radius of the earth; gamma, theta and phi represent a pitch angle, a roll angle and a course angle; w (t) and V (t) are respectively system noise and measurement noise;
discretizing a continuous system state equation and a measurement equation:
the state equation is as follows: xk＝Φk,k-1Xk-1+Γk-1Wk-1
The measurement equation is as follows: zk＝HkXk+Vk
Wherein, XkIs a system state vector, ZkFor the measurement vector, a measurement matrix HkH (t), the system noise matrix Γk-1＝G(tk) Δ t, state transition matrix Φk,k-1＝I+F(tk) Δ t (sampling time t)kK, the sampling period Δ t is 1), I is the identity matrix, Wk-1Is a system noise vector, VkTo measure the noise vector; wk-1And VkIs an uncorrelated white gaussian noise sequence, and the mean and variance are respectively:
E{Wk}＝0,E{WkWj T}＝Qkδkj,E{Vk}＝0,E{VkVj T}＝Rkδkj，cov{WiVj}＝0；
e, cov are shown separatelyMean and variance, QkIs a system noise variance matrix; rkTo measure the noise variance matrix, δkjIs a delta function;
step two: according to a conventional Kalman filtering algorithm, establishing a fuzzy self-adaptive filtering algorithm:
firstly, updating the state one-step predicted value and the mean square error thereof, and utilizing k moment measurement information ZkAnd one-step prediction
Figure GDA0003215722720000066
Calculating an innovation sequence rkSolving a gain matrix and a filtering equation;
and (3) one-step prediction:
Figure GDA0003215722720000067
predicting the mean square error:
Figure GDA0003215722720000071
gain matrix:
Figure GDA0003215722720000072
the innovation sequence is as follows:
Figure GDA0003215722720000073
the filter equation:
Figure GDA0003215722720000074
filtering mean square error: pk＝(I-KkHk)Pk/k-1
Wherein the content of the first and second substances,
Figure GDA0003215722720000075
for the state vector X in the filtering processkThe estimated amount of (a) is,
Figure GDA0003215722720000076
for one-step prediction of state, KkFor filter gain, PkBeing a covariance matrix of the filtering errors, Pk/k-1For prediction error covariance matrix, rkIs an innovation sequence, and I is an identity matrix;
step three: determining input and output parameters of the fuzzy inference system:
the theoretical value defining the variance of the residual is
Figure GDA0003215722720000077
And the actual value of the residual variance is
Figure GDA0003215722720000078
Two Fuzzy Inference Systems (FIS) were designed, both in single input single output mode. Ratio ROR of theoretical value to actual valuekAs input to the fuzzy inference system 1, the output is αk(ii) a Difference DOR between theoretical value and actual valuekB is used as the input of the fuzzy inference system 2;
theoretical value of residual variance:
Figure GDA0003215722720000079
actual value of residual variance:
Figure GDA00032157227200000710
ratio of theoretical value to actual value:
Figure GDA00032157227200000711
difference between theoretical value and actual value:
Figure GDA00032157227200000712
wherein the content of the first and second substances,
Figure GDA00032157227200000713
to average the latest N residual vector variances (N is empirically chosen and generally between 10 and 30, and mainly plays a smoothing role), i0＝k-N+1；riNamely a residual sequence; tr (-)) Indicating tracing the matrix;
step four: designing a fuzzy inference system:
the input of the fuzzy inference system 1 is the ROR found in the previous stepkOutput is alphakFuzzification is carried out on input and output, membership functions of the input and output are both triangular membership functions, a gravity center method is adopted for defuzzification, the membership functions are shown in figure 3, and the fuzzy control rule is as follows:
If RORk∈D(Decrease),thenαk∈D(Decrease)
If RORk∈M(Maintain),thenαk∈M(Maintain)
If RORk∈I(Increase),thenαk∈I(Increase)
the input of the fuzzy inference system 2 is DOR obtained in the last stepkAnd b is output, the input and the output are fuzzified, membership functions of the input and the output are both triangular membership functions, the fuzzification adopts a gravity center method, the membership functions are shown in figure 4, and the fuzzy control rule is as follows:
If DORk∈N(Negtive),then b∈I(Increase)
If DORk∈Z(Zero),then b∈M(Maintain)
If DORk∈P(Positive),then b∈I(Increase)
when the model is accurate, the model is,
Figure GDA0003215722720000081
and
Figure GDA0003215722720000082
the values of (A) and (B) are approximately equal, RORkIs near 1, adaptively adjusting the coefficient
Figure GDA0003215722720000083
So that
Figure GDA0003215722720000084
And maintained unchanged. If the measurement noise is increased by a small amount, then
Figure GDA0003215722720000085
With consequent increase in RORkIncreasing, adaptively adjusting coefficients
Figure GDA0003215722720000086
Increasing (at this point, b is smaller controlled by FIS2, preventing oscillation), then
Figure GDA0003215722720000087
And adjusting to a reasonable value. If the increase of the measurement noise is large, then
Figure GDA0003215722720000088
With consequent increase in RORkIncreasing, adaptively adjusting coefficients
Figure GDA0003215722720000089
Increasing (at this point, b is increased by FIS2 in appropriate amounts to reduce the transition time), then
Figure GDA00032157227200000810
And adjusting to a reasonable value. The process of measuring noise reduction is similar to the process of augmentation. A schematic block diagram of the fuzzy adaptive filter is shown in fig. 1.
Step five: updating the measurement noise estimation value:
updating the measurement noise estimation value, wherein the specific expression is as follows:
measuring a noise estimation value:
Figure GDA00032157227200000811
wherein the content of the first and second substances,
Figure GDA00032157227200000812
representing the measured noise estimate, alpha, of the k-th stepkRepresenting measurement noise
Figure GDA00032157227200000813
B is an exponential adjustment coefficient, representing the adjustment to alphakThe degree of magnification of. If b is>1, represents an amplification of αkTo pair
Figure GDA00032157227200000814
The adjustment function of (2) so that when the noise variation is measured,
Figure GDA00032157227200000815
the true value can be quickly approximated in a small number of steps. If b is<1, represents reduction of αkTo pair
Figure GDA00032157227200000816
The regulating action of (1). b is too large, which may result in
Figure GDA00032157227200000817
Carrying out small amplitude oscillation by taking the real measured noise value as the center; b too small value will result in
Figure GDA00032157227200000818
The transition time to adjust to the true value is slightly longer.
Step six: the filtering process is repeatedly performed:
and continuously repeating the second, third, fourth and fifth steps, carrying out online estimation on the measured noise by using a self-adaptive filtering method, and correcting the navigation information output by the strapdown inertial navigation system in real time until the SINS/GPS integrated navigation process is finished.
It will be understood that modifications and variations can be made by persons skilled in the art in light of the above teachings and all such modifications and variations are intended to be included within the scope of the invention as defined in the appended claims.
The method has the advantages that the method can be verified through Matlab and FUZZY tool box simulation experiments, the initial model is kept unchanged in the interval of 0-250 s, and the measurement noise is changed to 9 times of the initial value in the interval of 251-500 s. And respectively simulating the two-section process by using a conventional Kalman filtering method and a filtering method provided by the invention. The simulation curve is shown in fig. 5. The simulation parameters in the SINS/GPS integrated navigation system are set as follows:
the initial position is longitude 120 degrees and latitude 60 degrees, the initial speed is east 14m/s and north 14m/s, and the initial northeast balance table angle error2 ', 5 ', respectively, the initial velocity error is 0.01m/s and the INS initial longitude and latitude errors are 2 '. The sampling interval is 1s and the simulation time is 500 s. The simulation starting conditions are as follows: p0＝diag[22,22,52,0.012,0.012,22,22]，R0＝diag[0.22,0.22,22,22]。
Simulation results show that when the traditional Kalman filtering is adopted, the error is small when the random noise initial model is kept unchanged for 0-250 s. After 251-500 s, the random noise model changes suddenly, the variance of the measured noise changes, the initial measured noise variance value is still used for filtering, and the filtering result is subjected to larger fluctuation along with the increase of the noise and even possibly diverges. By adopting the filtering method, when the noise mutation is measured in the second half section, the filtering process is not obviously changed, the waveform is stable, and the position error is kept at a lower level. The fuzzy self-adaptive filtering method provided by the invention can be well adapted to the working state of the carrier in an uncertain environment by adjusting the variance matrix of the measured noise on line. Even if the system measurement noise changes, the estimation result of the filter cannot be influenced, and the method still has high estimation precision and good adaptability and has wide engineering application prospect.

Claims (1)

1. An unmanned ship combined navigation method based on fuzzy adaptive filtering is characterized by comprising the following steps:
the method comprises the following steps: establishing a system state equation and a measurement equation:
in an SINS/GPS combined navigation mode, an SINS is used as a main navigation system to establish a state equation of the system, longitude and latitude errors, speed errors and misalignment angles are selected as state variables, and a difference value of speed and position provided by the GPS and the SINS is used as a measurement variable to establish a measurement equation of the combined navigation system;
the state equation is as follows: xk＝Φk,k-1Xk-1+Γk-1Wk-1
The measurement equation is as follows: zk＝HkXk+Vk
Wherein, XkIs a system state vector, ZkFor measuring the vector, HkFor measuring the matrix,. phik,k-1Being a state transition matrix, Γk-1Is a system noise matrix, Wk-1Is a system noise vector, VkTo measure the noise vector; wk-1And VkIs an uncorrelated white gaussian noise sequence, and the mean and variance are respectively: e { Wk}＝0,E{WkWj T}＝Qkδkj,E{Vk}＝0,E{VkVj T}＝Rkδkj，cov{WiVj0; in the formula, QkIs a system noise variance matrix; rkTo measure the noise variance matrix, δkjIs a delta function;
step two: according to a conventional Kalman filtering algorithm, establishing a fuzzy self-adaptive filtering algorithm:
firstly, updating the state one-step predicted value and the mean square error thereof, and utilizing k moment measurement information ZkAnd one-step prediction
Figure FDA0003215722710000011
Calculating an innovation sequence rkSolving a gain matrix and a filtering equation;
and (3) one-step prediction:
Figure FDA0003215722710000012
predicting the mean square error:
Figure FDA0003215722710000013
gain matrix:
Figure FDA0003215722710000014
the innovation sequence is as follows:
Figure FDA0003215722710000015
the filter equation:
Figure FDA0003215722710000016
filtering mean square error: pk＝(I-KkHk)Pk/k-1
Wherein the content of the first and second substances,
Figure FDA0003215722710000017
for the state vector X in the filtering processkThe estimated amount of (a) is,
Figure FDA0003215722710000018
for one-step prediction of state, KkFor filter gain, PkBeing a covariance matrix of the filtering errors, Pk/k-1For prediction error covariance matrix, rkIs an innovation sequence, and I is an identity matrix;
step three: determining input and output parameters of a fuzzy inference system;
when input and output parameters of the fuzzy inference system are determined, the theoretical value of the residual variance is set as
Figure FDA0003215722710000019
And the actual value of the residual variance is
Figure FDA0003215722710000021
Designing two fuzzy inference systems which are in a single-input single-output mode, and comparing the ratio ROR of a theoretical value and an actual valuekAs input to the first fuzzy inference system, the output is αk(ii) a Difference DOR between theoretical value and actual valuekB is used as the input of a second fuzzy inference system;
theoretical value of residual variance:
Figure FDA0003215722710000022
actual value of residual variance:
Figure FDA0003215722710000023
ratio of theoretical value to actual value:
Figure FDA0003215722710000024
difference between theoretical value and actual value:
Figure FDA0003215722710000025
wherein the content of the first and second substances,
Figure FDA0003215722710000026
to average the latest N residual vector variances, i0＝k-N+1；riNamely a residual sequence; tr (-) denotes tracing the matrix;
step four: designing a fuzzy inference system (comprising two subsystems):
the input of the first fuzzy inference system is the ROR found in the previous stepkOutput is alphakFuzzifying input and output, wherein membership functions of the input and output are both triangular membership functions, and the defuzzification adopts a gravity center method;
the input of the second fuzzy inference system is DOR obtained in the last stepkB, fuzzifying the input and the output, wherein membership functions of the input and the output are both triangular membership functions, and the fuzzification adopts a gravity center method;
step five: updating the measurement noise estimation value:
from alpha found in the previous stepkAnd b, updating the measurement noise estimation value, wherein the specific expression is as follows:
measuring a noise estimation value:
Figure FDA0003215722710000027
wherein the content of the first and second substances,
Figure FDA0003215722710000028
indicating the measured noise estimate of the k-th step,αkrepresenting measurement noise
Figure FDA0003215722710000029
B is an exponential adjustment coefficient, representing the adjustment to alphakThe degree of magnification of;
step six: the filtering process is repeatedly performed:
and continuously repeating the second, third, fourth and fifth steps, carrying out online estimation on the measured noise by using a self-adaptive filtering method, and correcting the navigation information output by the strapdown inertial navigation system in real time until the SINS/GPS integrated navigation process is finished.
CN201810082656.0A 2018-01-29 2018-01-29 Unmanned ship integrated navigation method based on fuzzy adaptive filtering Active CN108490472B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810082656.0A CN108490472B (en) 2018-01-29 2018-01-29 Unmanned ship integrated navigation method based on fuzzy adaptive filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810082656.0A CN108490472B (en) 2018-01-29 2018-01-29 Unmanned ship integrated navigation method based on fuzzy adaptive filtering

Publications (2)

Publication Number Publication Date
CN108490472A CN108490472A (en) 2018-09-04
CN108490472B true CN108490472B (en) 2021-12-03

Family

ID=63343845

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810082656.0A Active CN108490472B (en) 2018-01-29 2018-01-29 Unmanned ship integrated navigation method based on fuzzy adaptive filtering

Country Status (1)

Country Link
CN (1) CN108490472B (en)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108844540A (en) * 2018-09-11 2018-11-20 北京机械设备研究所 A kind of adaptive filter method of combination covariance and Sage-Husa filtering technique
CN109443355B (en) * 2018-12-25 2020-10-27 中北大学 Visual-inertial tight coupling combined navigation method based on self-adaptive Gaussian PF
CN109974695B (en) * 2019-04-09 2022-08-26 东南大学 Robust adaptive filtering method of surface ship navigation system based on Krein space
CN110187282B (en) * 2019-06-03 2021-09-10 珠海东帆科技有限公司 Battery state of charge estimation method and estimation device
CN112556721B (en) * 2019-09-26 2022-10-28 中国科学院微电子研究所 Method and system for calibrating random error of navigation device filter
CN111044049B (en) * 2019-12-30 2022-07-22 东南大学 Improved UKF algorithm for aligning unmanned ship under severe sea conditions
CN112946641B (en) * 2021-01-27 2022-12-09 北京航空航天大学 Data filtering method based on relevance of Kalman filtering innovation and residual error
CN113932815B (en) * 2021-10-19 2023-07-18 北京京航计算通讯研究所 Robustness optimization Kalman filtering relative navigation method, device, equipment and storage medium

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010117147A (en) * 2008-11-11 2010-05-27 Seiko Epson Corp Method and device of calculating out position
CN102096086A (en) * 2010-11-22 2011-06-15 北京航空航天大学 Self-adaptive filtering method based on different measuring characteristics of GPS (Global Positioning System)/INS (Inertial Navigation System) integrated navigation system
CN102654404A (en) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 Method for improving resolving precision and anti-jamming capability of attitude heading reference system
CN102829777A (en) * 2012-09-10 2012-12-19 江苏科技大学 Integrated navigation system for autonomous underwater robot and method
CN103956956A (en) * 2014-05-13 2014-07-30 北京理工大学 Method for estimating status of brushless direct current motor based on extended kalman filter
CN103983996A (en) * 2014-05-09 2014-08-13 北京航空航天大学 Tight-integration adaptive filtering method of resisting to outliers of global positioning system,
CN104714188A (en) * 2015-03-31 2015-06-17 桂林电子科技大学 Method and system for estimating measured noise variance matrix matched battery state of charge (SOC)
CN106199652A (en) * 2016-06-24 2016-12-07 南京理工大学 A kind of self adaptation vector tracking method of GPS
CN107621264A (en) * 2017-09-30 2018-01-23 华南理工大学 The method for adaptive kalman filtering of vehicle-mounted micro- inertia/satellite combined guidance system

Family Cites Families (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8131463B2 (en) * 2003-12-02 2012-03-06 Gmv Aerospace And Defence, S.A. GNSS navigation solution integrity in non-controlled environments
US8229163B2 (en) * 2007-08-22 2012-07-24 American Gnc Corporation 4D GIS based virtual reality for moving target prediction
US8165742B2 (en) * 2008-11-14 2012-04-24 Robert Bosch Gmbh System and method for compensating sensor signals
CN101825903B (en) * 2010-04-29 2014-11-19 哈尔滨工程大学 Water surface control method for remotely controlling underwater robot
EP4198926A1 (en) * 2012-05-10 2023-06-21 President And Fellows Of Harvard College Method and apparatus for automatically discovering, characterizing, classifying and semi-automatically labeling animal behavior and quantitative phenotyping of behaviors in animals
CN102779238B (en) * 2012-08-09 2015-05-27 北京航空航天大学 Brushless DC (Direct Current) motor system identification method on basis of adaptive Kalman filter
CN103454665A (en) * 2013-08-26 2013-12-18 哈尔滨工程大学 Method for measuring double-difference GPS/SINS integrated navigation attitude
CN104062672A (en) * 2013-11-28 2014-09-24 哈尔滨工程大学 SINSGPS integrated navigation method based on strong tracking self-adaptive Kalman filtering
CN103759742B (en) * 2014-01-22 2017-04-05 东南大学 Serial inertial navigation nonlinear alignment method based on fuzzy adaptivecontroller technology
CN104112079A (en) * 2014-07-29 2014-10-22 洛阳理工学院 Fuzzy adaptive variational Bayesian unscented Kalman filter method
KR101698682B1 (en) * 2015-08-26 2017-01-23 매그나칩 반도체 유한회사 Method and Apparatus of correcting output value of terrestrial magnetism sensor
CN105737828B (en) * 2016-05-09 2018-07-31 郑州航空工业管理学院 A kind of Combinated navigation method of the joint entropy Extended Kalman filter based on strong tracking
CN106933106B (en) * 2016-05-26 2020-02-14 哈尔滨工程大学 Target tracking method based on fuzzy control multi-model algorithm
CN106199580B (en) * 2016-07-01 2018-08-14 中国人民解放军海军航空工程学院 A kind of Singer model refinement algorithms based on fuzzy inference system
CN106203698A (en) * 2016-07-11 2016-12-07 国网青海省电力公司 A kind of photovoltaic generation Forecasting Methodology based on Unscented kalman filtering and neutral net
CN107179081A (en) * 2017-06-23 2017-09-19 南京康派电子有限公司 A kind of transim
CN107290688B (en) * 2017-08-24 2019-08-09 合肥工业大学 A kind of lithium battery SOC estimation method based on adaptive fuzzy Kalman filtering

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010117147A (en) * 2008-11-11 2010-05-27 Seiko Epson Corp Method and device of calculating out position
CN102096086A (en) * 2010-11-22 2011-06-15 北京航空航天大学 Self-adaptive filtering method based on different measuring characteristics of GPS (Global Positioning System)/INS (Inertial Navigation System) integrated navigation system
CN102654404A (en) * 2011-03-02 2012-09-05 浙江中科无线授时与定位研发中心 Method for improving resolving precision and anti-jamming capability of attitude heading reference system
CN102829777A (en) * 2012-09-10 2012-12-19 江苏科技大学 Integrated navigation system for autonomous underwater robot and method
CN103983996A (en) * 2014-05-09 2014-08-13 北京航空航天大学 Tight-integration adaptive filtering method of resisting to outliers of global positioning system,
CN103956956A (en) * 2014-05-13 2014-07-30 北京理工大学 Method for estimating status of brushless direct current motor based on extended kalman filter
CN104714188A (en) * 2015-03-31 2015-06-17 桂林电子科技大学 Method and system for estimating measured noise variance matrix matched battery state of charge (SOC)
CN106199652A (en) * 2016-06-24 2016-12-07 南京理工大学 A kind of self adaptation vector tracking method of GPS
CN107621264A (en) * 2017-09-30 2018-01-23 华南理工大学 The method for adaptive kalman filtering of vehicle-mounted micro- inertia/satellite combined guidance system

Also Published As

Publication number Publication date
CN108490472A (en) 2018-09-04

Similar Documents

Publication Publication Date Title
CN108490472B (en) Unmanned ship integrated navigation method based on fuzzy adaptive filtering
CN108197350B (en) Unmanned ship speed and uncertainty estimation system and design method
Liu Ship adaptive course keeping control with nonlinear disturbance observer
Kraus Wave glider dynamic modeling, parameter identification and simulation
Pascoal et al. Ocean wave spectral estimation using vessel wave frequency motions
CN107179693B (en) Robust adaptive filtering and state estimation method based on Huber estimation
CN104316025B (en) System for estimating height of sea wave based on attitude information of ship
CN109407671B (en) Under-actuated unmanned ship target surrounding controller structure and design method
CN111897225B (en) Fuzzy self-adaptive output feedback control method and system for intelligent ship autopilot system
Randeni P et al. Parameter identification of a nonlinear model: replicating the motion response of an autonomous underwater vehicle for dynamic environments
CN111930124A (en) Fuzzy self-adaptive output feedback finite time control method and system for intelligent ship autopilot system
Brezoescu et al. Wind estimation for accurate airplane path following applications
CN107315348B (en) Position and posture control method of unmanned underwater vehicle based on punishment type wavelet network
Casado et al. Identification of nonlinear ship model parameters based on the turning circle test
Jaroś et al. Data fusion of GPS sensors using particle Kalman filter for ship dynamic positioning system
Zhang et al. Real-time disturbances estimating and compensating of nonlinear dynamic model for underwater vehicles
CN113467231A (en) Unmanned ship path tracking method based on sideslip compensation ILOS guidance law
Caiti et al. Fusing acoustic ranges and inertial measurements in AUV navigation: The Typhoon AUV at CommsNet13 sea trial
Li et al. The application of an extended Kalman filter in the dynamic positioning system
Peng et al. Extended state observer design for autonomous surface vehicles using position-yaw measurements
Zhang et al. An underwater SINS/DVL integrated system outlier interference suppression method based on LSTM-EEWKF
Subchan et al. Estimation of hydrodynamic coefficients using unscented kalman filter and recursive least square
Al-sudany et al. Comparison of Adaptive Fuzzy EKF and Adaptive Fuzzy UKF for State Estimation of UAVs Using Sensor Fusion
Haugaløkken et al. Adaptive Generalized Super-Twisting Tracking Control of an Underwater Vehicle
Zhai et al. AOA Based CKF Adaptive Algorithm for AUV Navigation

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