CN118258389A - A combined navigation method based on LTE/GNSS/INS/OD multi-sensor fusion - Google Patents

Abstract

The invention belongs to the field of integrated navigation, and particularly relates to a vehicle-mounted integrated navigation method based on multi-sensor fusion, which adopts multiple sensors, including: GNSS, SOP (based on LTE), ODO and IMU; the measurement equation of the Kalman filter is updated according to the information of the sensors, including: GNSS input speed and position information, SOP input position information, ODO input speed information; the state equation of the Kalman filter updates the posture information according to the measurement value of the inertial measurement unit; the obtained posture, speed and position information are input into the Kalman filter, and the Kalman filter generates a single integrated navigation result; the invention provides an effective solution for solving the limitations of GNSS positioning.

Description

A combined navigation method based on LTE/GNSS/INS/OD multi-sensor fusion

Technical Field

The invention belongs to the field of integrated navigation, and in particular relates to a vehicle-mounted integrated navigation method based on multi-sensor fusion.

Background technique

In-vehicle navigation plays an important role in modern life, and precise positioning technology has become the key to its realization. The positioning performance of traditional GNSS (Global Navigation Satellite System) degrades in complex environments such as urban canyons, which has led to the demand for more positioning technologies. The wide coverage of Long Term Evolution (LTE) networks provides an opportunity to use LTE signals as signals of opportunity (SOP) for positioning. The signal of opportunity positioning method does not require additional hardware and can use existing communication signals for positioning, which is cost-effective and convenient. In-vehicle combined positioning technology integrates multi-source information such as global navigation satellite systems, cellular signals, inertial measurement units, etc. to improve positioning accuracy and system robustness. In special scenarios such as urban canyons, high-rise buildings, and tunnels, traditional positioning systems are prone to failure, while in-vehicle combined positioning systems are more adaptable.

Current vehicle navigation usually uses a single GNSS system, and some use a combination of GNSS and an inertial navigation system (INS). However, in complex environments, GNSS may fail for a long time, and using INS alone will cause divergence in the results. Some solutions also use the vehicle odometer to suppress errors, but the one-dimensional forward odometer speed has limited error correction capabilities and cannot effectively initialize the vehicle's starting position.

Summary of the invention

In view of the problems existing in the prior art, the present invention proposes an integrated navigation method based on LTE/GNSS/INS/ODO multi-sensor fusion, and realizes dynamic position estimation of integrated navigation based on the Error-State Kalman Filter (ESKF) of Kalman filtering. It effectively combines GNSS, SOP, INS and odometer (Odometer, ODO) signals, which can improve the accuracy, robustness and availability of the positioning system, and bring significant improvements and application potential to the fields of vehicle navigation, autonomous driving, intelligent transportation, etc.

The purpose of the present invention is to solve the problem of poor accuracy and low robustness of the vehicle-mounted positioning and navigation system in a complex GNSS environment.

In the present invention, the distance information between the base station and the carrier is estimated by using the SOP signal to estimate the position information of the carrier, so as to replace the GNSS positioning information. And according to the working conditions of the GNSS and SOP signals, the measurement input of the filter is switched to achieve a combined positioning with high robustness in multiple scenarios. The present invention is based on the design of the combined navigation and positioning system of the error state Kalman filter, and is designed for the state model and the measurement model in the combined navigation system.

Technical solutions

An integrated navigation method based on LTE/GNSS/INS/ODO multi-sensor fusion, using multiple sensors, including: GNSS, SOP (based on LTE), ODO and IMU;

The measurement equation of the Kalman filter is updated based on the information from the sensors, including: GNSS input speed and position information, SOP input position information, and ODO input speed information;

The state equation of the Kalman filter updates the pose information based on the Inertial Measurement Unit (IMU) measurements;

The obtained posture, velocity and position information are input into the Kalman filter, which generates a single combined navigation result;

At the same time, there is a feedback loop between the Kalman filter and the state equation. The output result of the Kalman filter can be used for the prior estimation of the next moment, thereby correcting the posture information.

Specifically, a loosely combined Kalman filter is used, and the processing steps are as follows:

When GNSS works normally, accurate absolute coordinate observation values can be input. The measurement equation is composed of GNSS signal and SOP signal, so the measurement vector is set to z = [z _gnss , z _sop ].

When the GNSS fails to work properly, the input odometer speed value is used as the measurement vector, and the measurement equation is set to z=[z _odo ,z _sop ] in combination with the SOP signal.

When both of them cannot work properly, set the measurement equation z = [ _zodo ].

The IMU's measurement value is used as a state variable to output information related to the posture, which together with the measurement equation constitutes a part of the Kalman filter. The position is estimated and corrected through the filter, and finally a more accurate position result is output.

Beneficial effects of the present invention:

(1) Improving positioning accuracy and stability: By introducing LTE signals as opportunity signals, the present invention overcomes the problem of limited positioning accuracy of global navigation satellite systems in complex environments such as urban canyons, and provides a more reliable and stable positioning solution for vehicle-mounted navigation systems.

(2) Enhance the adaptability of the positioning system: The present invention adopts combined navigation technology to integrate multi-source information such as the global navigation satellite system, cellular signals, and inertial measurement units, so that the positioning system performs better in complex scenarios such as urban canyons, high-rise buildings, and tunnels, thereby improving the adaptability and availability of the system.

BRIEF DESCRIPTION OF THE DRAWINGS

Fig. 1 is a model diagram of the vehicle-mounted integrated navigation method of the present invention;

FIG2 is a structural diagram of a cellular positioning receiver according to an embodiment of the present invention;

FIG3 is a diagram of a printed circuit board according to an embodiment of the present invention;

FIG4 is a physical diagram of an embedded positioning system according to an embodiment of the present invention;

FIG5 is a comparison diagram of the errors of the simulation results of the positioning system according to an embodiment of the present invention;

FIG. 6 is a diagram showing an error actually measured by the integrated navigation system according to an embodiment of the present invention.

Detailed ways

The following is an explanation of the embodiments of the present invention by specific specific embodiments. Those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification, but the features of this invention are not limited to this embodiment. In order to provide a deep understanding of the present invention, many specific details will be included in the following description. The present invention can also be implemented without using these details. In addition, in order to avoid confusion or blurring the key points of the present invention, some specific details will be omitted in the description. It should be noted that, in the absence of conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other.

In order to make the objectives, technical solutions and advantages of the present invention more clear, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings.

As shown in Figure 1, a combined navigation method based on LTE/GNSS/INS/ODO multi-sensor fusion uses multiple sensors, including: GNSS, SOP, ODO and IMU;

The measurement equation of the Kalman filter is updated based on the information from the sensors, including: GNSS input speed and position information, SOP input position information, and ODO input speed information;

The state equation of the Kalman filter updates the pose information based on the IMU measurements;

The obtained posture, velocity and position information are input into the Kalman filter, which generates a single combined navigation result;

At the same time, there is a feedback loop between the Kalman filter and the state equation. The output result of the Kalman filter can be used for the prior estimation of the next moment, thereby correcting the posture information.

Specifically, a loosely combined Kalman filter is used, as follows:

When GNSS works normally, accurate absolute coordinate observation values can be input. The measurement equation is composed of GNSS signal and SOP signal, so the measurement vector is set to z = [z _gnss , z _sop ].

When the GNSS fails to work properly, the input odometer speed value is used as the measurement vector, and the measurement equation is set to z=[z _odo ,z _sop ] in combination with the SOP signal.

When both of them cannot work properly, set the measurement equation z = [z _odo ].

The IMU's measurement value is used as a state variable to output information related to the posture, which together with the measurement equation constitutes a part of the Kalman filter. The position is estimated and corrected through the filter, and finally a more accurate position result is output.

1. State equation

Furthermore, the state prediction equation of the Kalman filter is x(k+1|k)=F(k)x(k).

Where x is a state variable, which consists of two parts: a 15-dimensional state variable constructed with IMU measurements and a 2N-dimensional state variable constructed with cellular signal clock errors, that is, x = [x _imu , x _clk ]. Specifically:

1) 15-dimensional state variables constructed using IMU measurements

Among the 15-dimensional state variables, the first 9 dimensions are the errors of attitude, velocity, and position information, among which δθ, δφ, δψ are the errors of roll angle, pitch angle, and heading angle in the integrated navigation frame; the reference xyz axis coordinate system is set to the east-north-sky coordinate system, δv _E , δv _N , δv _U are the velocity errors corresponding to the reference coordinate system; δL, δλ, δh are the three-dimensional position errors; the last 6 dimensions are εx,εy,εz are the three-dimensional error values of the accelerometer and gyroscope along the main axis. The error is composed of the difference between the IMU measurement value and the estimated value.

Further describing the Kalman filter one-step prediction matrix is

in It is the direction cosine matrix from the navigation system to the vehicle system, indicating the direction of the vector in the navigation system in the vehicle system.

It is the antisymmetric matrix of the IMU angular accelerometer, which is obtained by multiplying the direction cosine matrix with the gyroscope zero bias.

Indicates the conversion of the navigation coordinate system from meters to longitude and latitude. RM represents the meridian curvature radius, i.e. the curvature radius of north-south movement. RN represents the normal curvature radius, i.e. the curvature radius of east-west movement. L is the latitude of the current position.

The effect of gravity acceleration due to altitude is eliminated.

F _gg and F _aa are the inverse values of the correlation time between the gyroscope and accelerometer, which are used to eliminate the influence of time on the deviation between the two.

Set the process noise matrix corresponding to the IMU to in, are the power spectral density (PSD) of gyroscope random noise, accelerometer random noise, gyroscope dynamic bias, and accelerometer dynamic bias, respectively.

2) 2N-dimensional state variables constructed with cellular signal clock error

The clock error state vector of the onboard receiver is set as

Where δt _r is the clock offset, which represents the offset of the receiver clock relative to the reference clock. Clock offset is the accumulated error of the clock relative to the true time.

The receiver clock drift represents the frequency deviation of the receiver clock, that is, the rate of change of the clock. Clock drift describes the change of clock deviation over time.

The clock error state vector of N _sop base stations is set as in

x _clk,sop,n is the clock error state vector of the nth SOP.

The dynamic expressions of x _clk,r and x _clk,sop,n at time k+1 are:

x _clk,r (k+1)＝F _clk x _clk,r (k)+ω _clk,r (k),

x _clk,sop,n (k+1)＝F _clk x _clk,sop,n (k)+ω _clk,sop,n (k).

F _clk represents the prediction matrix of the SOP signal, which describes how the clock offset and clock drift evolve from one moment to the next. The expression of the prediction matrix corresponding to one base station is as follows:

Design Clock Process Noise Matrix It consists of the power spectrum of the continuous-time equivalent process noise of the receiver clock offset and drift.

2. Measurement equation

The measurement equation of integrated navigation is composed of the measurement values of the global navigation satellite system, cellular signals and odometer. The design of the measurement equation is adaptively adjusted according to whether each sensor is working properly. When the GNSS is working properly, accurate absolute coordinate observations can be input, so the measurement vector is set to z = [z _gnss , z _sop ]. When the GNSS is not working properly, the input odometer speed value is used as the measurement vector, and the measurement vector is set to z = [z _odo , z _sop ] in combination with the SOP signal. When both are not working properly, the measurement equation z = [z _odo ].

δz _tdoa,1-n ＝δz _sop,1 -δz _sop,n .

The measured variable δz _tdoa is composed of the residual, which is the difference between the current observed time difference of arrival (TDOA) from different base stations to the receiving station and the TDOA value predicted at the previous moment. N _sop is the number of available cellular signal base stations. Due to the use of TDOA technology, we choose the base station with the strongest received signal as the reference station. The difference between its positioning result and the positioning results of other base stations is taken as the observation value. Its expression is as follows

δz _sop,n ＝z _sop,n -dis(k+1|k)

=z _sop,n -|| _psop,n -p _v (k+1|k)|| ₂ .

z _sop,n represents the pseudorange observed from the nth cellular base station. dis(k+1|k) represents the distance between the predicted position of the vehicle and its position at time k+1 (predicted from time k) relative to the nth cellular base station. || _psop,n - _pv (k+1|k)|| ₂ represents the two-norm of the three-dimensional distance difference, which is the square root of the sum of the squares of the vector components. The measurement covariance matrix It is the error value estimation of TDOA between the receiving station and different base stations.

z _odo is the 3D velocity error of the odometer, which is decomposed from a scalar into a 3D vector through the direction cosine. Its covariance matrix R _odo is defined as the product of the odometer error value and the direction cosine matrix, which is specifically expressed as

The six-dimensional GNSS observation error vector z _gnss = [δz _p ,δz _v ], and its covariance matrix

3. Covariance prediction process

Furthermore, the covariance prediction equation of the Kalman filter is

The function of the control matrix B is to introduce the influence of the error into the state estimation. The control matrix of the cellular signal clock error is the unit matrix. The control matrix expression corresponding to the IMU signal error is as follows:

4. Kalman filter update process

Then the Kalman filter gain is calculated as

K(k+1)＝P(k+1|k) ^HT (k)(H(k)P(k+1|k) ^HT (k)+R(k)) ^-1 ，

In the Kalman filter, the H observation matrix represents the mapping relationship between the state vector and the observation value, so as to facilitate the filter update. The H matrix is set by calculating the relationship between the observation vector and the state vector. Therefore, the setting of the observation matrix also depends on the working state of each sensor.

When GNSS is working well, set up the measurement matrix with GNSS and SOP

When GNSS does not work properly, the measurement matrix Combination of odometer and cellular signal.

When both GNSS and cellular signals cannot be received normally, the odometer signal can be used as the observation value.

According to the partial derivative chain rule, the Jacobian matrix is derived, and its value is

in

H _ODO and H _GNSS are set to

H _ODO =(0 _3×3 I _3×3 0 _3×3 0 _3×12 ),

The estimated value is corrected and updated using the H matrix calculated above and the measurement equation Z obtained through GNSS, SOP, and odometer signals. The update process is expressed as x(k+1)=x(k+1|k)+K(k+1)(z(k+1)-x(k+1|k)).

Finally, the covariance is updated

In summary, the prediction equation of the Kalman filter of the present invention is:

x(k+1|k)＝F(k)x(k)

The update equation is:

K(k+1)＝P(k+1|k)HTk)(H ⁽ k)P(k+1|k) ^HT (k)+R(k)) ⁻¹

x(k+1)＝x(k+1|k)+K(k+1)(z(k+1)-x(k+1|k))

5. Cellular positioning system implementation:

The LTE frame information of the commercial base station is received by three software defined radio peripherals (USRP) to communicate. The advantage is that the commercial base station signal is strong, has a long propagation distance, and has high signal quality, which is helpful for the construction of the vehicle navigation system. Figure 2 is the structure diagram of the cellular positioning receiver.

Select USRP device to build the receiver. Use LabVIEW program to simulate the receiver at the receiving end, connect USRP to receive the signal sent by the base station. Configure parameters such as frequency, sampling rate, gain, etc. to adapt to the characteristics of LTE signals.

Secondly, signal synchronization is performed to ensure correct signal sampling. Synchronization is achieved by looking for correlation detection of the primary and secondary synchronization signals in the LTE signal to determine the frame header position. The ID of the LTE cell is determined from the combination of the known primary synchronization signal (PSS) and secondary synchronization signal (SSS) sequences. After synchronization, the CP prefix in each LTE frame is deleted according to the determined frame header position to obtain the correct OFDM symbol.

After that, the synchronized LTE signal is demodulated, and the frequency domain signal is converted into a time domain baseband signal. According to the modulation mode of the LTE signal, the signal is demodulated accordingly, and the demodulated signal is separated into real and imaginary parts for subsequent processing.

Finally, for cellular signal delay extraction, the CRS pilot of the LTE downlink signal is used to estimate the channel frequency response, thereby estimating the delay parameters of the received LTE signal. Through channel modeling, channel estimation and channel impulse response analysis, the arrival time and location of the mobile terminal can be determined.

In order to output the positioning results in real time, the method of the present invention is implemented with an embedded system. The present invention develops a development board as the software carrier of the vehicle-mounted information-assisted integrated navigation system, uses an STM32 chip as a microprocessor to receive sensor data, and is equipped with the fusion algorithm of the present invention, which can output positioning information in real time. Since cellular signal positioning is not easy to embed, a computer is used alone to connect the receiver to collect LTE information, which is later fused with GNSS, INS, etc. As shown in Figures 3 and 4.

Fig. 5 is the simulation result of the inventive method, which is the root mean square error (RMSE) value of the single sensor SOP signal positioning and the different state combination positioning over time. The positioning accuracy of the positioning system based on ESKF is improved by about 60% compared with the single LTE positioning. And because IMU provides angle information, the odometer is similar to GNSS in suppressing the system error after speed decomposition. Therefore, no matter whether GNSS works normally, the positioning accuracy of the system does not change significantly, proving that the robustness of the designed combined navigation model is higher.

Figure 6 is a graph showing the variation of the error RMSE value over time measured using the self-developed development board of the present invention. The different curves in the figure respectively represent the error values of the system in longitude, latitude and overall position direction. Compared with using LTE positioning alone, adding IMU as a state quantity not only smoothes the positioning trajectory, but also provides a more accurate direction angle for the decomposition of vehicle speed in a short time. The introduction of vehicle speed also limits the error divergence accumulated by IMU over time to a certain extent. The three work together to make the final positioning error result 3.2524m in longitude and 3.9656m in latitude.

It can be seen that the method of the present invention has strong robustness in complex scenes such as urban canyons, high-rise areas, tunnels, etc.

Although the present invention has been illustrated and described with reference to certain preferred embodiments of the present invention, it should be understood by those skilled in the art that the above contents are further detailed descriptions of the present invention in conjunction with specific embodiments, and it cannot be determined that the specific implementation of the present invention is limited to these descriptions. Those skilled in the art may make various changes in form and details, including making several simple deductions or substitutions, without departing from the spirit and scope of the present invention.

Claims (5)

1. The integrated navigation method based on LTE/GNSS/INS/ODO multi-sensor fusion is characterized by adopting a plurality of sensors and comprising the following steps: GNSS, SOP (LTE-based), ODO, and IMU;

The measurement equation of the Kalman filter is updated according to the information of the sensor, and the measurement equation comprises the following components: GNSS inputs speed and position information, SOP inputs position information, ODO inputs speed information;

The state equation of the Kalman filter updates pose information according to the measured value of the inertial measurement unit (Inertial Measurement Unit, IMU);

the obtained pose, speed and position information is input into a Kalman filter, and a single combined navigation result is generated by the Kalman filter;

meanwhile, a feedback loop exists between the Kalman filter and the state equation, and the output result of the Kalman filter can be used for prior estimation at the next moment, so that the pose information is corrected.

2. The integrated navigation method based on the LTE/GNSS/INS/ODO multi-sensor fusion according to claim 1, wherein the integrated navigation state variable x is composed of two parts: 15-dimensional state variables constructed with IMU measurements and 2N-dimensional state variables constructed with cellular signal clock errors, i.e., x= [ x _imu,x_clk ]; the method comprises the following steps:

1) 15-dimensional state variables constructed with IMU measurements

In the 15-dimensional state variables, the first 9 dimensions are the error amounts of the gesture, the speed and the position information, wherein delta theta, delta phi and delta phi are the error amounts of the roll angle, the pitch angle and the course angle in the combined navigation frame; the reference x-y-z axis coordinate system is set as an east-north-sky coordinate system, δv _E,δv_N,δv_U is the speed error amount of the corresponding reference coordinate system; δl, δλ, δh are three-dimensional position error amounts; rear 6 dimensionΕx, εy, εz are the three-dimensional error values of the accelerometer and gyroscope along the body axis; the error amount is formed by the difference value between the IMU measured value and the estimated value;

The state prediction equation of the kalman filter is x (k+ 1|k) =f (k) x (k), and further describes a one-step prediction matrix of the kalman filter;

Wherein the method comprises the steps of The direction cosine matrix from the navigation system to the vehicle system represents the direction of the vector in the navigation system in the vehicle system;

the anti-symmetric matrix of the IMU angular accelerometer is obtained by multiplying a direction cosine matrix by zero offset of a gyroscope;

The unit of the navigation coordinate system is converted from meters to longitudes and latitudes; where RM represents the radial radius of curvature, i.e. the radius of curvature of north-south motion, and RN represents the normal radius of curvature, i.e. the radius of curvature of east-west motion. L is the latitude of the current location;

The influence of gravity acceleration caused by the altitude is eliminated;

f _gg and F _aa are the inverse and negative values of the time associated with the gyroscope and the accelerometer, and are used for eliminating the influence of time on the deviation of the gyroscope and the accelerometer;

setting a process noise matrix corresponding to the IMU as Wherein,The power spectral densities of the random noise of the gyroscope, the random noise of the accelerometer, the dynamic bias of the gyroscope and the dynamic bias of the accelerometer are respectively;

2) 2N-dimensional state variables built with cellular signal clock errors

The vehicle-mounted receiver clock error state vector is set as follows

Wherein δt _r is the clock offset, representing the offset of the receiver clock relative to the reference clock; clock skew is the cumulative error of a clock relative to real time;

For receiver clock drift, the frequency offset of the receiver clock, i.e., the rate of change of the clock; clock drift describes the change in clock offset over time;

The clock error state vector of N _sop base stations is set to Wherein the method comprises the steps of

X _clk,sop,n is the nth SOP clock error state vector;

the dynamic expression of x _clk,r and x _clk,sop,n at time k+1 is

x_clk,r(k+1)＝F_clkx_clk,r(k)+ω_clk,r(k)，

x_clk,sop,n(k+1)＝F_clkx_clk,sop,n(k)+ω_clk,sop,n(k)；

F _clk denotes the prediction matrix of the SOP signal, describing how the clock offset and clock drift evolve from one time instant to the next, where the expression of one base station corresponding to the prediction matrix is as follows:

Design clock process noise matrix Consisting of a power spectrum of continuous time equivalent process noise of receiver clock bias and drift.

3. The integrated navigation method based on the combination of LTE/GNSS/INS/ODO as claimed in claim 1, wherein the measurement equation of the integrated navigation is composed of measurements of a global navigation satellite system, cellular signals and ODO, and the design of the measurement equation is adaptively adjusted according to whether each sensor works normally:

When the GNSS works normally, an accurate absolute coordinate observation value can be input, and a measurement vector is set as z= [ z _gnss,z_sop ];

When the GNSS can not work normally, the input speedometer is used as a measurement vector, and the measurement vector is set as z= [ z _odo,z_sop ] by combining with an SOP signal;

when the two cannot work normally, setting a measurement equation z= [ z _odo ];

Wherein,

The measurement variable delta z _tdoa is composed of residual quantity, and takes the difference between the arrival time of the current observation from the different base stations to the receiving station and the TDOA value of the current moment predicted at the last moment as the quantity measurement; n _sop is the number of available cellular signal base stations; because the TDOA technology is used, the base station with the maximum received signal strength is selected as a reference station, and the difference between the positioning result of the base station and the positioning results of other base stations is used as an observation value; the expression is as follows

δz_sop,n＝z_sop,n-dis(k+1|k)

＝z_sop,n-||p_sop,n-p_v(k+1|k)||₂

Where z _sop,n represents the pseudorange observed from the nth cell site, dis (k+1|k) represents the distance between the predicted position of the vehicle and its position at time k+1 (predicted from time k) relative to the nth cell site, |p _sop,n-p_v(k+1|k)||₂ represents the two norms of the three-dimensional distance difference, which is the square root of the sum of the squares of the components of the vector;

Measurement covariance matrix Estimating an error value of TDOA between a receiving station and different base stations;

z _odo is the three-dimensional velocity error of the odometer, decomposed from scalar into three-dimensional vectors by direction cosine; the covariance matrix R _odo is defined as the product of the odometer error value and the direction cosine matrix, and is expressed in detail as

Six-dimensional GNSS observation error vector z _gnss＝[δz_p,δz_v ], covariance matrix

4. The integrated navigation method based on LTE/GNSS/INS/ODO multi-sensor fusion of claim 1, wherein the covariance prediction equation of the Kalman filter is

The control matrix B is used for introducing the influence of errors into state estimation, the control matrix of the clock errors of the cellular signals is a unit matrix, and the expression of the control matrix corresponding to the IMU signal errors is as follows

5. The integrated navigation method based on the LTE/GNSS/INS/ODO multi-sensor fusion of claim 1, wherein the Kalman filtering updating process is as follows:

kalman filtering gain of

In the Kalman filter, an H observation matrix represents the mapping relation between a state vector and an observation value so as to facilitate filtering update; setting an H matrix by calculating the relation between the observation vector and the state vector; the setting of the observation matrix is thus also dependent on the operating state of the individual sensors;

When the GNSS is operating well, the measurement matrix is set by the GNSS and SOP

When GNSS is not working normally, the matrix is measuredThe device is formed by combining an odometer and a cellular signal;

When GNSS and cellular signals can not be normally received, the odometer signal can be used as an observed quantity independently, and at the moment

Deriving a jacobian matrix having a value of

Wherein the method comprises the steps of

H _ODO and H _GNSS are arranged as

H_ODO＝(0_3×3 I_3×3 0_3×3 0_3×12)，

The estimated value is corrected and updated, and the measurement equation Z obtained by GNSS, SOP and odometer signals is used, wherein the updating process is expressed as x (k+1) =x (k+ 1|k) +K (k+1) (Z (k+1) -x (k+ 1|k)).

Finally, updating the covariance