CN114111797A

CN114111797A - Kalman filter, IP core and chip for navigation based on FPGA

Info

Publication number: CN114111797A
Application number: CN202111441052.9A
Authority: CN
Inventors: 刘宁; 袁超杰; 沈凯; 苏中; 赵文江; 刘一康; 邓志红; 付梦印
Original assignee: Nanjing University of Science and Technology; Beijing Institute of Technology BIT; Beijing Information Science and Technology University
Current assignee: Nanjing University of Science and Technology; Beijing Institute of Technology BIT; Beijing Information Science and Technology University
Priority date: 2021-11-30
Filing date: 2021-11-30
Publication date: 2022-03-01
Anticipated expiration: 2041-11-30
Also published as: CN114111797B

Abstract

The invention discloses a Kalman filter, an IP core and a chip for navigation based on an FPGA. Wherein, this kalman filter includes: an input interface configured to receive a clock signal and a reset signal; a master module configured to calculate a state prior estimate, a state truth value, a measurement truth value, a Jacobian matrix, and a measurement matrix based on the clock signal and the reset signal; an output interface configured to output the state prior estimate, the state truth value, the measurement truth value, the Jacobian matrix, and the measurement matrix for state prediction and state update. The invention solves the technical problems of time-consuming development, higher cost and slower execution speed of the Kalman filter in the related technology.

Description

Kalman filter, IP core and chip for navigation based on FPGA

Technical Field

The invention relates to the field of computers, in particular to a Kalman filter, an IP core and a chip for navigation based on an FPGA.

Background

The traditional Kalman filtering implementation mode mainly comprises an ASIC (application specific integrated circuit), a DSP (digital signal processor) and the like, but the implementation modes have certain defects and have the problem of low speed.

In view of the above problems, no effective solution has been proposed.

Disclosure of Invention

The embodiment of the invention provides a Kalman filter, an IP core and a navigation chip based on an FPGA (field programmable gate array), which are used for at least solving the technical problems of time consumption, higher cost and lower execution speed of the Kalman filter in the related technology.

According to an aspect of an embodiment of the present invention, there is provided an FPGA-based kalman filter, including: an input interface configured to receive a clock signal and a reset signal; a master module configured to calculate a state prior estimate, a state truth value, a measurement truth value, a Jacobian matrix, and a measurement matrix based on the clock signal and the reset signal; an output interface configured to output the state prior estimate, the state truth value, the measurement truth value, the Jacobian matrix, and the measurement matrix for state prediction and state update.

In the embodiment of the invention, the state prior estimated value, the state true value, the measurement true value, the Jacobian matrix and the measurement matrix are calculated based on the clock signal and the reset signal, so that the technical problems of time consumption, high cost and low execution speed of the Kalman filter in the related technology are solved.

Drawings

FIG. 1 is a schematic structural diagram of an FPGA-based Kalman filter according to an embodiment of the invention;

FIG. 2 is a schematic structural diagram of a navigation chip according to an embodiment of the present invention;

FIG. 3A is a flow chart of a filtering method of a Kalman filter in accordance with an embodiment of the present invention;

FIG. 3B is a schematic diagram of another Kalman filter according to an embodiment of the invention;

FIG. 3C is a schematic diagram of state flow of a state machine according to an embodiment of the present invention;

FIG. 4 is a flow chart of a method of IP core encapsulation for a Kalman filter in accordance with an embodiment of the present invention.

Fig. 5 is a diagram of normalized geomagnetic signals, according to an embodiment of the present invention;

fig. 6 is a sine curve diagram of a normalized geomagnetic signal according to an embodiment of the present invention.

Detailed Description

Example 1

According to an embodiment of the present invention, there is provided an FPGA-based kalman filter, as shown in fig. 1, comprising an input interface 12, a main module 14 and an output interface 16.

An input interface 12 configured to receive a clock signal and a reset signal; a main module 14 configured to calculate a state prior estimate, a state true value, a measurement true value, a jacobian matrix, and a measurement matrix based on the clock signal and the reset signal; an output interface 16 configured to output the state prior estimation value, the state true value, the measurement true value, the jacobian matrix and the measurement matrix for state prediction and state update.

In the embodiment, the Kalman filter based on the FPGA solves the technical problems of time consumption, high cost and low execution speed of development of the Kalman filter in the related technology through the structure. In this embodiment, the kalman filter implemented based on the FPGA employs a hardware parallel algorithm, and has better expandability, faster speed, and stronger real-time performance compared to the kalman filter implemented by other conventional processors.

In one exemplary embodiment, the kalman filter further includes: the device comprises a plurality of fixed-point multiplication modules, a matrix inversion module, a state updating module and a matrix multiplication module.

A plurality of fixed-point multiplication modules configured to calculate, based on control of the main module, a Kalman gain K and a true value Z at the time of updating a state optimum estimation value from an input clock signal and input data_kEstimated value Z_preMultiplication of the corresponding bits.

A matrix inversion module configured to calculate a matrix division operation in a Kalman gain from the clock signal, a reset signal, and a matrix input to output an inverse matrix based on control of the main module.

A state updating module configured to calculate a jacobian matrix at time K, a measurement matrix, a state prior estimate, a measurement prior estimate, a state true value, and a measurement true value according to a clock signal, a reset signal, an input state estimate, and an input measurement true value based on control of the main module.

A matrix multiplication module configured to calculate a product of the Jacobian matrix and the metrology matrix based on control of the master module.

In the embodiment, by arranging the plurality of fixed-point multiplication modules, the advantages of expandability, instantaneity and the like of the FPGA are fully utilized, and a faster and more accurate implementation method compared with the traditional Kalman filter is provided, so that the development period is short, the cost is low, and the execution speed is higher.

The embodiment of the application also provides an IP core comprising the Kalman filter based on the FPGA. The designed Kalman filter is packaged into an IP core, and the IP core can be directly called when in use, so that the Kalman filtering algorithm can be more conveniently realized.

Example 2

According to an embodiment of the present invention, there is also provided a chip for navigation, as shown in fig. 2, the chip for navigation includes a kalman filter 22 and a processor 24.

The structure of the kalman filter 22 is the same as that of the kalman filter 22 in embodiment 1, and details thereof are omitted.

A processor 24 configured to: under the condition that an aircraft is in a first stage, measuring the rotating speed of the aircraft by utilizing geomagnetism to estimate the flight track of the aircraft, wherein the first stage is a stage in which an inertia measuring device is failed; starting the inertial sensor under the condition that the aircraft is in a second phase, and acquiring the working state of the inertial sensor based on the rotating speed of the aircraft measured in the terrestrial magnetism and the rotating speed of the aircraft acquired by the inertial sensor so as to estimate the flight track of the aircraft, wherein the second phase is a phase that the rotating speed of the aircraft measured in the terrestrial magnetism is lower than a set threshold value; performing initial alignment based on aircraft data acquired by a satellite to estimate a flight trajectory of the aircraft when the aircraft is in a third phase, wherein the third phase is a phase in which the aircraft data can be acquired through the satellite, and the aircraft data comprises attitude, position and speed information of the aircraft; and under the condition that the aircraft is in a fourth stage, estimating the flight track of the aircraft by using the Kalman filter based on the rotation speed, attitude, position and speed information of the aircraft measured by the geomagnetism, wherein the fourth stage is a stage after the aircraft reaches a ballistic vertex.

In an exemplary embodiment, the processor 24 is further configured to: through the geomagnetic measurement, the rotation speed and the rotation difference value of the aircraft are estimated, the inertial information of the aircraft is obtained through the inertial sensor, and real-time ballistic measurement and ballistic prediction are carried out through the satellite and the nominal ballistic to determine the state quantity and the observed quantity of the Kalman filter; and performing zero offset error correction on the output of the inertial sensor based on the state quantity and the observed quantity of the Kalman filter, and performing trajectory error correction on the real-time trajectory measurement and trajectory prediction of the satellite.

In an exemplary embodiment, the processor 24 is further configured to: acquiring the maximum value, the minimum value and the average value of the geomagnetic signal of the previous window; on the basis of the maximum value, the minimum value and the average value of the acquired geomagnetic signals of the previous window, normalization processing is carried out on the geomagnetic signal points acquired in the current window one by one, and geomagnetic signal points after the current-time geomagnetic signal normalization are continuously acquired; and analyzing whether zero points appear or not based on the geomagnetic signal points obtained after normalization, taking the acquired time values of two adjacent zero points as characteristic values of data analysis, calculating the periods of the sine waves in the time when the two current zero points appear based on the characteristic values, and calculating the rotating speeds of the aircraft in the two current times based on the periods of the sine waves in the two current times.

In one exemplary embodiment, the processor is further configured to: acquiring an original value of inertial navigation data within a sampling time Ts, and normalizing the original value of the inertial navigation data; carrying out gravity elimination processing on the original value of the inertial navigation data after normalization processing; calculating the period of a sine wave in the sampling time Ts based on the original value of the inertial navigation data subjected to gravity elimination processing, determining the time setting range of the next sampling time Ts based on the period of the sine wave in the sampling time, and taking the next sampling time as the sampling time Ts; wherein the inertial navigation data includes a rotational speed of the aircraft acquired by the inertial sensor.

The chip for navigation in the embodiment can more accurately navigate the aircraft through the structure.

Example 3

According to the embodiment of the invention, the invention further provides the Kalman filter based on the FPGA.

The essence of the kalman filter is a recursive prediction-correction method, which is limited to linear systems only. The Extended Kalman Filter (EKF) is one of the more effective processing methods to solve the problem of non-linear state estimation.

In order to increase the calculation speed, the following method is adopted to design the extended kalman filter.

First, a nonlinear system considering gaussian white noise is introduced, and the system can be represented by equations (1), (2):

X_k＝f(X_k-1)+W_k-1 (1)

Z_k-1＝h(X_k-1)+V_k-1 (2)

in the above two formulae, X_kIs the state vector of the system at time k, Z_k-1Is the measurement vector of the system at the time of k-1; f (x) and h (x) in the formulas (1) and (2) represent a system nonlinear state function and a measurement function respectively; w_k-1And V_k-1The Noise interference of the system in the process and measurement can be considered, namely White gaussian Noise (White gaussian Noise) of the kalman filtering algorithm, and the covariance of the White gaussian Noise is Q_K-1And R_k-1。

In the kalman filtering method provided in this embodiment, the method may include two steps:

(1) and (6) predicting. The state of the current time k is predicted by the posterior estimate value of the previous time, i.e., the time (k-1), thereby obtaining an equation of the posterior estimate value of the current time.

(2) And (6) updating. As shown in fig. 3A, the estimation value of the prediction phase is corrected by measuring the value at the current time, thereby obtaining the posterior estimation value at the current time.

State estimation at known time k-1

And estimate the variance P_k-1Time, the non-linear function f (X)_k-1) Combining a non-linear function f (X)_k) In that

After the first-order Taylor expansion, the state equation can be simplified as follows:

wherein F_k-1After Taylor expansion

Is the jacobian matrix at time k-1.

The time update equation of the kalman filter can be expressed by equations (4) and (5).

The time updating equation, namely the prediction phase equation of the Kalman filtering algorithm, is used for calculating a state variable prior estimation value and an error covariance prior estimation value at the current moment according to the state estimation value at the previous moment (k-1 moment). Wherein

The posterior state estimate, also called the optimal estimate, representing the time k-1 is the updated result.

The prior state estimation value at the time k is the result of k time predicted according to the optimal estimation at the last time (time k-1) and is the result of a prediction equation. P_k-1Is the a posteriori estimated covariance at time k-1

Covariance of (e) representing the uncertainty of the state.

Representing the prior estimated covariance at time k, i.e.

The covariance of (a). Q_kThen it represents the process excitation noise at time kThe acoustic covariance, i.e. the covariance of the system process, is used to represent the error between the state transition matrix and the actual process. The time update equation is also referred to as a prediction equation.

Will nonlinear function h (X)_k-1) In that

After the first-order Taylor expansion, the measurement equation can be simplified as follows:

wherein H_kAfter Taylor expansion

Is the measurement matrix at time k.

The measured prediction error covariance matrix is:

the cross-covariance matrix between the states and measurements is:

the state update equations of the kalman filter can be expressed by equations (9), (10), (11).

Where equation (9) is a state gain matrix. Equation (10) is a state estimation value at time k, and equation (11) is a state estimation error covariance matrix, which represents uncertainty of a state. H_kIs the transformation matrix of the state variables to measures (observations) at time k, representing the relationship of the state and observations connected. Z_kIs a measurement (observation) value representing the filtered input. K_kIs a state gain matrix, also known as kalman gain or kalman coefficient. R_kThe covariance of the measurement noise at time k, i.e., the covariance of the system process, is represented as the error between the state transition matrix and the actual process. The measurement update equation is also referred to as a correction equation.

The Kalman filter based on the FPGA provided by the embodiment of the application can realize the extended Kalman filtering method. Fig. 3B is a schematic structural diagram of an FPGA-based kalman filter according to an embodiment of the present application, and as shown in fig. 3B, the FPGA-based kalman filter includes: main module 14, matrix inversion module 322, fixed point multiplication module 324, status update module 326, and matrix multiplication module 328.

In this embodiment, an extended kalman filter algorithm is implemented by the FPGA, and the operation is decomposed into four types of matrix operations, i.e., "add", "subtract", "multiply", "invert".

There are 8 fixed-point multiplication modules 322(qmult), a state update module 326, a matrix inverse module 322, and a sequential matrix multiplication module 328 added to the master module 14. Wherein 7 CORDIC (trigonometric function calculation)

modules

3261, 2 CORDIC _ atan (inverse trigonometric function calculation) modules 3262, 11 qmult (fixed point multiplier with 18 bit integer and 30 bit fractional)

modules

3263, 3 sqrt (square-on-square calculation) modules 3264, 4 qdiv (divider calculation)

modules

3265 and 4 ROM _ drive (register) modules 3266 are added in the state _ update module; a matrix _ inverse module 322 is added with 4 qdiv (division calculation) modules and 2 called multiplier IP cores mult; the sequentiai _ matrix _ multiplex module 328 is added with 4 coeffient _ calc modules 3282. Each vector multiply computation module 3282 is internally provided with two fixed-point multiplier modules 32821, and each fixed-point multiplier is internally provided with a called multiplier IP core mult 328211.

In the state _ update module 326, the algorithm for calculating the jacobian matrix and the measurement matrix at the time k is implemented by using each module as follows:

let the discrete state model be:

X_1，k+1＝X_2，ksinX_1，k+0.1k+W_1，k (12)

X_2，k+1＝X_1，k+cos²X_2，k-0.1k+W_2，k (13)

the measurement model is as follows:

in the formula, X_1，k+1The first variable, X, representing the state vector of the system at time k +1_2，kA second variable, X, representing the state vector of the system at time k_1，kA first variable representing the state vector of the system at time k, k representing the time value, X_2，k+1A second variable, Z, representing the state vector of the system at time k +1_1，k+1The first variable, Z, representing the measurement vector of the system at time k +1_2，k+1A second variable, W, representing the measurement vector of the system at time k +1_1，k、W_2，k、V_1，k+1、V_2，k+1Each of which is 0 expected white noise, and is independent of each other and independent of the state, the k-time jacobian matrix and the measurement matrix are respectively:

specific input parameters and output parameters of the respective modules will be described below.

In the kalman.v main module, the input parameters are: clk (clock signal), reset (reset signal); the output parameters are as follows: xhat1, xhat2 (state-optimal estimated value), xpre _ sample1, xpre _ sample2 (state-prior estimated value), xr _ sample1, xr _ sample2 (state true value), zr _ sample1, zr _ sample2 (measurement true value), kmatrix11, kmatrix10, kmatrix01, kmatrix00 (kalman gain matrix entries), F (jacobian matrix), and H (measurement matrix).

The main module calls four big modules, which are respectively:

1. the matrix _ inverse matrix inversion module is used for calculating matrix division operation involved in Kalman gain, and 2 mult modules and 4 qdiv divider modules are called by the matrix _ inverse matrix inversion module.

In the matrix _ inverse matrix inversion module, input parameters include: clk (clock signal), reset (reset signal), matrix _ in (matrix input); the output parameters are as follows: matrix _ out (inverse matrix output).

2. 8 qmult (fixed point multiplier with 18-bit integer and 30-bit decimal) modules for calculating Kalman gain K and true value Z involved in updating state optimal estimation value_kEstimated value Z_preMultiplication of the corresponding bits.

In the qmult module, the input parameters are: clk (clock signal), a, b (input data a and b); the output parameters are as follows: o _ result (output result).

3. The state _ update state update module is used for the operations of state true values Xr1 and Xr2, measurement true values Zr1 and Zr2, state prior estimation values Xpre1 and Xpre2, measurement prior estimation values Zpre1 and Zpre2, a jacobian matrix F and a transpose F _ transpose thereof, and a transpose H _ transpose thereof. And meanwhile, calling a rom module for generating Gaussian white noise. According to trigonometric function calculation, inverse trigonometric function calculation, multiplication calculation and division calculation required in operation, the following modules are called:

7 CORDIC (trigonometric function calculation) modules, 2 CORDIC _ atan (inverse trigonometric function calculation) modules, 11 qmult (fixed point multiplier) modules, 3 sqrt (open square calculation) modules, 6 qdiv (division calculation) modules and 4 ROM _ drive (register) modules.

In the state _ update state update module, the input parameters are as follows: clk (clock signal), reset (reset signal), x1, x2, (input state estimate, inputs xhat1 and xhat2 at call time), xc1, xc2 (input measurement true value, inputs xr _ sample and xr _ sample2 at call time); outputting parameters: xe1, xe2 (state prior estimate), ze1, ze2 (measurement prior estimate), xr1, xr2 (state true), zr1, zr2 (measurement true), F _ transpose (jacobian matrix F and its transpose), H _ transpose (measurement matrix H and its transpose).

In the CORDIC (trigonometric function calculation) module, the input parameters are: clock (input clock) X _ start, Y _ start (input X and Y coordinates, where Y is fixed to 0, X is fixed to 2 to the Q power divided by 1.647), angle _ radial (input angle); the output parameters are as follows: sine (output sine), cosine (output cosine).

In the CORDIC _ atan (inverse trigonometric function calculation) module, the input parameters are: clock, X _ start, Y _ start (input X coordinate, input Y coordinate); the output parameters are as follows: atan _ f (output arctangent value).

In the sqrt module, the input parameters are: i clk, (clock), rst (reset), i _ vacuum (enable signal), data _ in (data input), and output parameters are: o _ vaild (output enable), data _ out (output data), data _ remaining (remainder).

In the qdiv (division calculation) module, the input parameters are: i _ dividend _ o (input of 32-bit dividend), i _ divsor _ o (input of 32-bit divisor), i _ start (calculation start flag), i _ clk (input clock); the output parameters are as follows: o _ quotient (output 32-bit division result), o _ complete (operation completion flag bit), o _ overflow (overflow flag bit)

In the ROM _ drive (register) module, the input parameters are: clk _100M (clock), Rst _ n (reset), En (enable), and ROM _ q (output).

4. The sequentiai _ matrix _ multiplex module is added with 4 coeffient _ calc modules (used for 2X2 matrix operation). With two qmult modules within each vector multiply compute module.

In the sequential _ matrix _ multiplex module, the input parameters are: clk (clock signal), reset (reset signal), a, B (input matrices a and B); the output parameters are as follows: c (output matrix C).

The state machine flow diagram is shown in fig. 3C. The state of state 1 is computer _ F _ x _ P _ pre, which represents the product of the covariance matrix and the jacobian matrix; the state of state 2 is computer _ F _ x _ P _ pre _ x _ F _ transpose, which represents the product of the last state result calculated and the jacobian transpose matrix; the state of state 3 is computer _ P _ pre _ x _ H _ transpose, which represents the product of the calculated prior error and the H transpose matrix; the state of state 4 is computer _ H _ x _ P _ pre _ x _ H _ transit, which represents the result of the last result left-multiplying the measurement matrix; the state 5 state is computer _ K, representing the calculated kalman gain; the state 6 is computer _ K _ x _ H, which represents the product of the calculated kalman gain and the measurement transpose matrix; the state of state 7 is computer _ P, and represents the calculated error covariance, i.e., the updated value of the error covariance; the state of the state 8 is state _ wait _ K, which represents state waiting; the state 9 is state _ prepare _ K, which means that the state machine enters the preparation stage and can perform the next operation at any time.

Example 4

According to the embodiment of the invention, an IP core packaging method of a Kalman filter based on an FPGA is provided. In this embodiment, a Vivado software is used to encapsulate an IP core, and a specific flow is shown in fig. 4, which includes the following steps:

and step S402, modifying the project. Step S404, whether the engineering function simulation or the time sequence exhibition is completely correct. And (4) executing the step (S406) under the condition of creating and ensuring that the engineering function simulation or the time sequence simulation is completely correct, otherwise, returning to the step (S402). Step S406, a blank IP is created. In step S408, the storage position is selected. Step S410, IP is packaged. IP core encapsulation of the project.

Step S412, determine whether the IP core is successfully encapsulated.

If an error occurs in the process of simulating or encapsulating the IP core, the project needs to be rechecked and modified, i.e., the process goes back to step S402. After the IP core is successfully packaged, only one Block Design file needs to be created in Vivado software, and the packaged IP core can be added in the project. And reconfiguring the pins and adding a constraint file to complete the calling of the IP core.

In the embodiment, the designed Kalman filter is packaged into the IP core, and the IP core can be directly called when in use, so that the Kalman filtering algorithm can be more conveniently realized.

Example 5

According to an embodiment of the present application, a chip for navigation is provided, where the chip includes a processor, and the processor includes a first stage module, a second stage module, a third stage module, and a fourth stage module.

The first stage module is configured to measure the rotating speed of the aircraft by using geomagnetism to estimate the flight track of the aircraft under the condition that the aircraft is in a first stage, wherein the first stage is a stage in which an inertia measurement device fails; the second stage module is configured to activate the inertial sensor and obtain an operating state of the inertial sensor to estimate a flight trajectory of the aircraft based on the rotation speed of the aircraft measured in the terrestrial magnetism and the rotation speed of the aircraft obtained by the inertial sensor, if the aircraft is in a second stage, wherein the second stage is a stage in which the rotation speed of the aircraft measured in the terrestrial magnetism is lower than a set threshold value; the third stage module is configured to perform an initial alignment based on aircraft data acquired by a satellite to estimate a flight trajectory of the aircraft if the aircraft is in a third stage, wherein the third stage is a stage in which the aircraft data including attitude, position, and velocity information of the aircraft can be acquired by the satellite. In an exemplary embodiment, the initial alignment may be performed by motion information of the aircraft acquired by the satellite, satellite velocity resolution angle information, nominal ballistic information, inertial information. And the fourth stage module is configured to construct a Kalman filter based on the rotation speed, attitude, position and speed information of the aircraft measured in the geomagnetic measurement and estimate the flight track of the aircraft by using the Kalman filter under the condition that the aircraft is in a fourth stage, wherein the fourth stage is a stage after the aircraft reaches a ballistic vertex.

In one exemplary embodiment, the first stage module is further configured to: acquiring an original value of a geomagnetic signal in a current window of the geomagnetic signal, and normalizing the original value of the geomagnetic signal in the current window; and calculating the period of the sine wave in the current window based on the original value of the geomagnetic signal after the normalization processing, and calculating the current rotating speed of the aircraft based on the period of the sine wave in the current window.

In one exemplary embodiment, the second stage module is further configured to: confirming the working state of a flying axis gyroscope based on the rotation speed of the aircraft measured by the geomagnetism and the rotation speed of the aircraft detected by the gyroscope; judging the working state of the accelerometer through the ballistic parameters; wherein the inertial sensor comprises the gyroscope and the accelerometer.

In one exemplary embodiment, the second stage module is further configured to: acquiring an original value of the inertial navigation data within the sampling time Ts, and normalizing the original value of the inertial navigation data; carrying out gravity elimination processing on the original value of the inertial navigation data after normalization processing; calculating the period of the sine wave in the sampling time Ts based on the original value of the inertial navigation data after gravity elimination processing, determining the time setting range of the next sampling time Ts based on the period of the sine wave in the sampling time, and taking the next sampling time as the sampling time Ts.

In one exemplary embodiment, the fourth phase module is further configured to: through the geomagnetic measurement, the rotation speed and the rotation difference value of the aircraft are estimated, the inertial information of the aircraft is obtained through the inertial sensor, and real-time ballistic measurement and ballistic prediction are carried out through the satellite and the nominal ballistic to determine the state quantity and the observed quantity of the Kalman filter; and performing zero offset error correction on the output of the inertial sensor based on the state quantity and the observed quantity of the Kalman filter, and performing trajectory error correction on the real-time trajectory measurement and trajectory prediction of the satellite. In one exemplary embodiment, the state quantities include a velocity error, a position error, an attitude error, and a gyro zero offset; the observed quantity comprises position, speed, pitching, yawing and flying axial gyroscope zero offset.

The estimation apparatus in this embodiment can implement the estimation method in the above embodiments, and is not described herein again. The structure of the kalman filter in this embodiment is the same as that of the kalman filter in the above embodiment, and details are not repeated here.

In the embodiment, when the rotating speed is lower than the inertial navigation starting threshold value, the inertial system is started, the three-axis gyroscope and the three-axis accelerometer are powered on to work, and the condition that an inertial device fails due to a high dynamic environment is effectively avoided. The precision and speed of initial alignment of the system are improved by combining satellite positioning data and bound ballistic parameters.

Example 6

According to the embodiment of the invention, the chip for navigation comprises a processor and a Kalman filter. Wherein the processor is capable of performing the following method:

1. and collecting data.

Relevant data is collected from scalar geomagnetism in a first stage, gyroscopes, accelerometers in a second stage, and nominal trajectory and satellites in a third stage for corresponding navigation.

2. And constructing a Kalman function, and performing error correction and attitude calculation.

And after entering the fourth phase, entering a combined navigation phase.

1) And constructing a Kalman function.

Firstly, selecting the error quantity of a system as a state variable: position error Δ P, velocity error Δ v, and attitude error

The accelerometer zero offset error delta a and the gyro zero offset error delta omega establish a state equation as follows:

wherein, Δ P_kAnd Δ P_k+1Position errors at the time k and the time k +1 respectively; Δ V_kAnd Δ V_k+1The speed errors at the time k and the time k +1 respectively;

and

attitude errors at the time k and the time k +1 respectively; Δ a_kAnd Δ a_k+1Zero offset errors of the accelerometer at the time k and the time k +1 respectively; Δ ω_kAnd Δ ω_k+1The gyroscope zero offset errors at the time k and the time k +1 respectively; b is₁、B₂The proportional coefficients of the accelerometer zero offset error and the gyroscope zero offset error; w_k·a、W_k·ωThe random system dynamic noises are respectively the errors of an accelerometer and a gyroscope at the moment k, and the mean value and the variance of the random system dynamic noises meet the requirements

Is a zero mean white noise sequence; k denotes the time of day, at is the time,

for the variation matrix, n denotes a navigation coordinate system and b denotes a carrier coordinate system. St is an antisymmetric matrix.

Writing the above equation as the standard equation of state X_k+1＝f[X_k，k]+W_kThe form is as follows:

wherein F is a state translation matrix; i is_3*3An identity matrix of 3x 3;

is a coordinate transformation matrix.

Secondly, a measurement equation is established.

Establishing a measurement equation Z_k+1＝h[X_k+1，k+1]Wherein, the quantity is measured as the position and speed of the aircraft, and is specifically shown as the following formula:

wherein Z is_k+1Measured for the time k +1, h is the observation matrix, X_k+1And k is the state vector of the system at the moment k +1, and k is the current calculation moment. The measured system noise, mean and variance in the formula (4) satisfy

Is a zero mean white noise sequence; i is a 3x3 identity matrix.

Finally, an Extended Kalman Filter (EKF) is constructed.

P_k，k-1＝f_k，k-1P_k-1f^f _k，k-1+Q_k-1

In the formula P_k，k-1Predicting covariance for the state; p_kEstimating a covariance for the state; k_kIs a Kalman gain matrix; q_kA covariance matrix which is the system noise; r_kIs a covariance matrix of the measured noise. The first two formulas are the state prediction part, and the last three formulas are the update part.

2) And (5) carrying out attitude calculation.

Using acceleration and gyroscope information (inertial navigation data) acquired from a sensor for SINS data calculation to obtain position, speed and attitude information;

where x _ h (1: 6) is position and velocity. x _ h (7), x _ h (8) and x _ h (9) are roll angle, pitch angle and yaw angle, and are calculated by adopting a formula (11); the acc _ t is inertial navigation data after gravity is eliminated; rb2t is a direction cosine matrix turning Euler angle; ts is the sampling time.

3) And (5) carrying out error correction.

And correcting inertial navigation data by using the satellite data and the geomagnetic signal.

Calculating an error amount:

K＝(P*H′)/(H*P*H′+R)

z＝-x_h(1：6，k)

dx＝K*z

wherein z is a difference value of data between the GPS and the SINS, K is Kalman filtering gain, P is a state equation covariance matrix, H is an observation matrix, H' is a transposed matrix of the observation matrix, R is a measured noise covariance, and x _ H is a navigation information matrix solved by the inertial navigation system;

and (3) correcting navigation data:

P_k＝[I-K_kH_k]P_k，k-1

wherein x _ h is a navigation information matrix solved by the inertial navigation system, dx is the disturbance obtained, R32 is R moment third row and second column data, R33 is R matrix third row and third column data, R31 is R matrix third row and first column data, R21 is R matrix second row and first column data, R11 is R matrix first row and first column data, q2dcm is quaternion-direction cosine matrix, quat_kIs a quaternion at the moment K, K is the current calculation time, K_kTo extend the Kalman gain, P_k.k-1A matrix is predicted for one step of the covariance of the system,

for transposing the observation matrix, H_kIn order to observe the matrix, the system,

respectively a covariance matrix of the measurement noise at the moment k and a mean value of the measurement noise; epsilon_kWhich is a residual error, is determined,

for the purpose of a one-step prediction of the state quantity of the system,

for the system state quantity at the moment k,

for the purpose of a one-step prediction of the state quantity of the system,

is the system state quantity at the last moment, phi_k.k-1For the one-step prediction matrix of the state transition,

is the mean value of the system noise, P_kIs the system covariance matrix at time k, I is the identity matrix,

covariance matrix, P, for system measurement noise_k，k-1A matrix is predicted for one step of the covariance of the system,

transpose matrix for one-step prediction of state transitions, P_k-1Is the covariance matrix of the system at the previous moment, Γ_k-1The array is driven for the system noise,

the covariance matrix of the system noise at the previous time instant,

is the transpose matrix of the system noise driving matrix, and T is time.

Wherein the content of the first and second substances,

covariance matrix of measurement noise at time k-1, ε_kWhich is a residual error, is determined,

is the transposed matrix of the residual, T is the matrix transposed sign, Hk is the observation matrix, P_k，k-1A matrix is predicted for one step of the covariance of the system,

in order to be a transpose of the observation matrix,

is the mean value of the measurement noise at time k-1, d_k＝(1-b)/(1-b^k+1) And (b is more than 0 and less than 1) is a forgetting factor which has the function of strengthening the function of filtering on new data.

3. And performing Kalman updating.

Updating the Kalman gain K:

K＝(P*H′)/（H*P*H′+R)

updating the filter state covariance matrix P:

P＝F*P*F′+G*Q*G′

P＝(P+P′)/2

cov(：，k)＝diag(P)

wherein P represents a filtering state covariance matrix, F represents a state transfer matrix, F ' represents a transposed matrix of the state transfer matrix, G represents a process noise gain matrix, Q represents, G ' represents a transposed matrix of the process noise gain matrix, P ' represents a transposed matrix of the filtering state covariance matrix, and cov (: k) represents data at time k on a diagonal of the state covariance matrix.

Update state transfer matrix F:

where E denotes a unit matrix of 15 × 15, and Rb2t denotes a quaternion-to-direction-cosine matrix in the case where the quaternion is not 0.

The navigation chip in the embodiment performs error correction on inertial navigation data by using the geomagnetic signal and the satellite data, and performs combined navigation in stages, so that the problem of inaccurate navigation in the related art is solved, and the navigation chip has the advantage of improving navigation accuracy.

Example 7

According to an embodiment of the present invention, there is provided another chip for navigation, including a processor and a kalman filter. The difference between the processor in this embodiment and the above embodiment is that normalization processing and smoothing processing are performed on data acquired by the inertial sensor, for example, accelerometer data, and other processing steps are the same as those in the above embodiment and are not described again here. The present embodiment will focus on the processing of inertial navigation data.

The processing of inertial navigation data by the processor in this embodiment includes the following steps:

1) a first sampling time is set.

And setting a time setting range of the first sampling time, and taking the first sampling time as the current sampling time. The time setting range of the first sampling time is set to 200 ms.

2) And acquiring the maximum value, the minimum value and the average value of the original values of the inertial navigation data in the current sampling time.

3) Abnormal data in the original value is removed.

4) And smoothing the original value of the processed inertial navigation data.

In one embodiment, the original value data may be smoothed after the anomalous data is removed, for example: taking the average every n points, i.e.

Wherein m is_sRepresenting the smoothed original value of the inertial sensor, so that the original value of the inertial sensor can be conveniently processed.

In other embodiments, this step may be omitted.

5) And normalizing the original value of the inertial navigation data after the smoothing processing.

Normalization processing is performed based on the following formula:

wherein Q_iTo normalize the processed data values, m_sIs the smoothed original value, w, of the inertial sensor in the current sampling time range_(i)ave、w_(i)maxAnd w_(i)minRespectively, the average, maximum and minimum values of the raw values of the inertial sensor over the sampling time range.

In an exemplary embodiment, w_(i)aveThe calculation process of (2) is as follows:

in order to enable the calculation result to be closer to the true value, wherein n represents the number of the original value points of the inertial sensor in two adjacent maximum value ranges in the sampling time; r represents the number of abnormal data of the original value of the inertial sensor in the sampling time; r represents the sum of the values of the anomaly data.

6) And calculating the rotating speed in the current sampling time.

The cycle of the sine wave in the sampling time can be obtained by the times x of the sine wave image passing through the zero point in the sampling time:

where t represents a sample time value.

Further calculating the rotation speed of the aircraft:

7) and navigating based on the calculated rotating speed.

Compared with other existing navigation technologies, the navigation technology provided by the embodiment uses the processed inertial navigation data for navigation, so that fewer interference factors are applied in the measurement process, and the navigation is more reliable, stable and accurate.

Example 8

According to an embodiment of the present invention, there is provided another chip for navigation, including a processor and a kalman filter. This embodiment is different from embodiment 7 in that the next sampling time is predicted based on the current sampling time, and other processing steps are the same as those in embodiment 7. The processor in this embodiment performs the following method to process inertial navigation data (e.g., accelerometer data).

1) A first sampling time is set.

And setting a time setting range of the first sampling time, and taking the first sampling time as the current sampling time.

3) Abnormal data in the original value is removed.

4) And smoothing the original value of the processed inertial navigation data.

5) And normalizing the original value of the smoothed inertial navigation data.

6) And calculating a sine period in the current sampling time, and calculating the current rotating speed based on the sine period.

Steps 1) to 6) are the same as steps 1) to 6) in embodiment 6, and are not described herein again.

7) Based on the current sampling time, a time setting range of the next sampling time is defined.

Defining a time setting range of a next sampling time as lambda/R based on a sine period of a sine wave of an inertial sensor signal within a current sampling time_iAnd λ is a multiple, which can be set according to practical conditions as long as it is ensured that the current sampling time range includes at least one sine cycle.

In step S708, navigation is performed based on the calculated rotation speed.

In this embodiment, the navigation chip predicts the duration of the next sampling time window based on the sine wave image of the sampling data in the current sampling time window, and can intercept the sine wave image of more complete sampling data, so that the sampling data can reflect the real situation more, and the navigation is more accurate.

Example 9

According to the embodiment of the invention, the invention also provides a chip for navigation. The navigation method of the chip for navigation in the first stage, the second stage and the third stage is basically the same as that of the embodiment, but the difference is in the fourth stage, and after the navigation data is acquired in the fourth stage, the chip for navigation not only depends on the navigation data for navigation, but also performs navigation based on the preset expected track.

The processor of the chip for navigation of the present embodiment is further configured to:

1) and constructing a combined navigation model. 2) And constructing an extended Kalman filtering algorithm. 3) And (5) carrying out attitude calculation. And based on the constructed integrated navigation model and the constructed extended Kalman filtering algorithm, error correction and attitude calculation are carried out. Steps 1) to 3) are the same as the steps of constructing the integrated navigation model, constructing the extended kalman filter algorithm, and calculating the attitude in embodiment 7, and are not described herein again.

4) The desired trajectory is calculated.

In the embodiment, the expected flight trajectory is introduced, and when the navigation is combined, the corrected inertial navigation data is considered, and the navigation is performed based on the expected flight trajectory.

For example, in the case where the satellite data is not updated, the aircraft is navigated using the navigation parameters using a spiral as an expected flight trajectory in the case where the satellite data is not updated; and under the condition that the satellite data is updated, taking the intersection point of the expected flight path and a reference circle under the condition that the satellite data is not updated and a straight line pointing to the center of the reference circle as a reference line, taking the straight line of which the focal point connecting line is parallel to the reference line as the expected flight path, based on a combined navigation mathematical model, carrying out error correction on inertial navigation data of the inertial sensor by using the satellite data, and carrying out inertial calculation by using the corrected inertial navigation data to obtain navigation parameters of the aircraft so as to navigate the aircraft.

Specifically, the center a of the reference circle may be represented as:

A＝E-r_Ae＝F-(r_A+r_EF)e＝(x_F-(r_A+r_EF)cosλ_F，y_F+(r_A+r_EF)sinλ_F)

wherein r is_AIs the radius of the reference circle, E is the intersection point of the expected track and the reference circle when satellite data is not detected, E is the direction vector, r_EFDistance of expected track for satellite data detection, x_FAnd y_FIs the current X-axis and Y-axis coordinates, λ, of the aircraft_FIs the angle of incidence.

Since the spiral trajectory passes the point of initiation (x)_B，y_B) Thus, the distance r of the desired track when the satellite data is detected_EFCan be expressed as:

wherein r is_EFLength of expected flight path for satellite data detection, x_FIs the current X-axis coordinate, y, of the aircraft_FAs the Y-axis coordinate, x, of the aircraft_BAs X-axis coordinate of the start-point, y_BFor the Y-axis coordinate, λ, of the actuating point_FIs the angle of incidence, θ_n1To desired inclination, λ_MIs the terminal angle, K is the coefficient.

Thereafter, the reference line is determined:

wherein, b_n2Is a polynomial constant term coefficient, x_FAn X-axis coordinate, y, being the center of the reference circle_FA Y-axis coordinate, x, being the center of the reference circle_BIs the X-axis coordinate, y, of said intersection_BIs the Y-axis coordinate, λ, of said intersection point_FK is a constant coefficient for the angle of incidence.

In the determination of r_EFAnd b_n2Then, it can be seen that the track points to the center a of the reference circle through the point E, and the slope is tan λ_FThe expected flight trajectory at the time of detection of satellite data can therefore be expressed as:

wherein, y_n2To output, b_n2Is a constant term coefficient.

5) And fitting the expected flight trajectory.

After the desired flight trajectory is calculated, the calculated desired flight trajectory is fitted based on the acquired inertial navigation data to navigate the aircraft.

Example 10

According to the embodiment of the invention, the invention also provides a chip for navigation. The chip for navigation in this embodiment is basically similar to the navigation chip in the above embodiments in structure and function, and the difference is that the processor of the chip for navigation is further configured to process the geomagnetic signal. In the present application, the geomagnetic signal is also referred to as geomagnetic data or geomagnetic information, and all the geomagnetic data is data collected by a geomagnetic sensor.

The existing rotation speed measuring scheme calculated through the geomagnetic signal belongs to a post-processing mode, the real-time performance is not high, and the window-drawing range (the first window) and the scale coefficient are uncertain, so the embodiment makes the following improvements on the rotation speed measuring method through the geomagnetic signal.

First of all a first time window w is defined₀The range of (1). Finding a first maximum value and a first minimum value in data corresponding to the received geomagnetic signal, and defining a first window w₀In the time range of

t₀＝2(t_0max-t_0min)

Wherein t is_0maxIs the point in time at which the first maximum of the data occurs, t_0minThe point in time at which the first minimum occurs. Since the rotation period of the aircraft is gradually increased in the flight process, the window w is provided₀The normalized data curve in the time range at least comprises two zero points.

Then to the window w₀And (5) normalizing the internal data.

At w₀Determining maximum value w of geomagnetic signal in range_0maxMinimum value w_0minAnd the average value w_0ave. The window w is obtained by calculation₀And (5) normalizing the processed data to obtain:

wherein m is₀Is the original value of the geomagnetic signal. The normalized geomagnetic signal is shown in fig. 5.

The sine curve of the normalized geomagnetic signal is shown in FIG. 6, t₀And t₁Respectively the time when the sinusoid passes through the characteristic points. Since the sampling frequency of the geomagnetic signal is fixed, the sinusoidal curve is composed of a number of discrete points, and the time interval between each point is constant. It is considered that the sampled value is not necessarily a characteristic value of a sine curve, and the characteristic value near the zero crossing point is more easily distinguished than the characteristic value near the peak and the trough. Therefore, the value near the extraction zero point is selected as the characteristic value of data analysis, so that the error caused by sampling is reduced, and the rotating speed of the aircraft is obtained more accurately.

Judging point t_nThe way of whether it is zero or not is: get t_nMultiplying the data values of the adjacent time interval sampling points on the left side and the right side, and if the obtained result is a negative value, t is_nIs zero. Through the window w_iThe middle image passes through two adjacent zero points t_inAnd t_i(n+1)The elapsed time, the period of the sine wave at time n can be derived:

T_in＝2(t_i(n+1)-t_in)

projectile rotational speed R_in：

By analogy, the current window w can be calculated_iWithin a sine wave period T of each time_inAnd projectile rotational speed R_inTherefore, the requirement of measuring the real-time performance of the rotating speed is met. Meanwhile, considering the situation that data waveform jitter occurs near the zero point, so as to calculate the abnormal rotating speed value (normally, the rotating speed interval is from minus 30 to plus 300), when the distance between the two zero points is too close, for example, smaller than the first distance threshold value, and the calculation is deviated, the group of zero points are deleted, so as to achieve the purpose of deleting the abnormal value.

Because the rotating speed of the projectile body gradually slows down or even reverses in the flying process, the next window w is set_i+1Is set to be lambda T_in(the value of λ can be determined according to the actual application requirement, in the present algorithm λ is 3), λ is the proportionality coefficient, i.e. the next window w_i+1Is the current window w_iBy a factor of lambda of the time of a single period of the last group of sinusoidal images, thereby ensuring that w_i+1Has at least one sinusoidal period. Then to w_i+1The data within the range is normalized, which yields:

in the formula: q_i+1For after treatmentThe data value of (a); m is_i+1Is w_i+1The original value of the data within the range; w is a_iave、w_imaxAnd w_iminAre respectively w_iMean, maximum and minimum values of data within the range.

I.e. by w_iCalculated last group period T_inAnd a rotational speed R_inTo set w_i+1In the time range of, then w_i+1Normalizing the data by using the maximum value, the minimum value and the average value of the data in the previous window in a set time range, and calculating the period T of each moment in the window_(i+1)nAnd a rotational speed R_(i+1)nAnd setting w as the last set of period and rotation speed in the window_i+2And repeating the steps until all data are calculated, and finally finishing the rotation speed calculation.

Example 11

According to the embodiment of the invention, the invention also provides a chip for navigation. The chip for navigation in this embodiment is basically similar to the navigation chip in the above embodiments in structure and function, and the difference is that the processor of the chip for navigation is further configured to process the geomagnetic signal.

The navigation chip is also configured to obtain the maximum value, the minimum value and the average value of the geomagnetic data of the previous window; based on the maximum value, the minimum value and the average value of the acquired geomagnetic data of the previous window, performing normalization processing on the current geomagnetic data point acquired in real time in the current window, and judging whether the current geomagnetic data point after the normalization processing is a zero point or not so as to find out two adjacent zero points in the current window; calculating the rotating speed of the flying body in two moments corresponding to the two adjacent zero points based on the two adjacent zero points; wherein the geomagnetic data is composed of a plurality of discrete geomagnetic data points.

The navigation chip is further configured to acquire geomagnetic data values of adjacent time interval sampling points on the left side and geomagnetic data values of adjacent time interval sampling points on the right side of the current geomagnetic data point after normalization processing; multiplying the geomagnetic data value of the adjacent time interval sampling point on the left side by the geomagnetic data value of the adjacent time interval sampling point on the right side to obtain a result value; and under the condition that the result value is less than zero, judging that the current geomagnetic data point after normalization processing is a zero point, otherwise, judging that the current geomagnetic data point is not the zero point. For example, the values of two moments corresponding to the two adjacent zero points are used as characteristic values of data analysis; calculating the periods of the sine waves corresponding to the two moments based on the characteristic values; and calculating the rotating speed of the flying object based on the period of the sine wave.

In a case that the current window is a first time window, the navigation chip is further configured to: finding a first maximum value and a first minimum value of the geomagnetic data in the geomagnetic data; demarcating a range for the first time window based on a time point at which the first maximum value occurs and a time point at which the first minimum value occurs; calculating a maximum value, a minimum value and an average value of the geomagnetic data in the first time window, and taking the calculated maximum value, minimum value and average value of the geomagnetic data in the first time window as the maximum value, minimum value and average value of the geomagnetic data in the previous window respectively.

The navigation chip is also configured to normalize the current geomagnetic data point obtained in real time in the current window based on the maximum value, the minimum value and the average value of the acquired geomagnetic data of the previous window, judge whether the normalized current geomagnetic data point is a zero point, and execute the step in a circulating manner until two adjacent zero points in the current window are found out; and judging whether the distance between the two adjacent zero points is smaller than a distance threshold value, if so, deleting the two adjacent zero points, acquiring the next group of two adjacent zero points, and if not, keeping the two adjacent zero points.

And the navigation chip also sets the time range of the next window based on the period corresponding to the last group of two adjacent zero points in the current window, and takes the next window as the current window. The time range of the next window is integral multiple of single cycle time of the last group of sinusoidal images of the geomagnetic data in the current window.

In the embodiment, the navigation chip performs error correction on the inertial navigation data by using the kalman filter and the satellite data, and fits the expected flight trajectory based on the corrected inertial navigation data, so that the problem of inaccurate navigation in the related technology is solved, and the navigation chip has the advantage of improving the navigation accuracy.

Claims

1. A Kalman filter based on FPGA, characterized by comprising:

an input interface configured to receive a clock signal and a reset signal;

a master module configured to calculate a state prior estimate, a state truth value, a measurement truth value, a Jacobian matrix, and a measurement matrix based on the clock signal and the reset signal;

an output interface configured to output the state prior estimate, the state truth value, the measurement truth value, the Jacobian matrix, and the measurement matrix for state prediction and state update.

2. The kalman filter according to claim 1, further comprising: a plurality of fixed-point multiplication modules configured to calculate, based on control of the main module, a Kalman gain K and a true value Z at the time of updating a state optimum estimation value from an input clock signal and input data_kEstimated value Z_preMultiplication of the corresponding bits.

3. The kalman filter according to claim 1, further comprising: a matrix inversion module configured to calculate a matrix division operation in a Kalman gain from the clock signal, a reset signal, and a matrix input to output an inverse matrix based on control of the main module.

4. The kalman filter according to claim 1, further comprising: a state updating module configured to calculate a jacobian matrix at time K, a measurement matrix, a state prior estimate, a measurement prior estimate, a state true value, and a measurement true value according to a clock signal, a reset signal, an input state estimate, and an input measurement true value based on control of the main module.

5. The Kalman filter of claim 4 further comprising: a matrix multiplication module configured to calculate a product of the Jacobian matrix and the metrology matrix based on control of the master module.

6. An IP core of a Kalman filter based on an FPGA, the IP core comprising: the FPGA-based Kalman filter of any one of claims 1 to 5.

7. A chip for navigation, comprising:

the FPGA-based Kalman filter of any one of claims 1 to 5;

a processor configured to:

under the condition that an aircraft is in a first stage, measuring the rotating speed of the aircraft by utilizing geomagnetism to estimate the flight track of the aircraft, wherein the first stage is a stage in which an inertia measuring device is failed;

starting the inertial sensor under the condition that the aircraft is in a second phase, and acquiring the working state of the inertial sensor based on the rotating speed of the aircraft measured in the terrestrial magnetism and the rotating speed of the aircraft acquired by the inertial sensor so as to estimate the flight track of the aircraft, wherein the second phase is a phase that the rotating speed of the aircraft measured in the terrestrial magnetism is lower than a set threshold value;

performing initial alignment based on aircraft data acquired by a satellite to estimate a flight trajectory of the aircraft when the aircraft is in a third phase, wherein the third phase is a phase in which the aircraft data can be acquired through the satellite, and the aircraft data comprises attitude, position and speed information of the aircraft;

and under the condition that the aircraft is in a fourth stage, estimating the flight track of the aircraft by using the Kalman filter based on the rotation speed, attitude, position and speed information of the aircraft measured by the geomagnetism, wherein the fourth stage is a stage after the aircraft reaches a ballistic vertex.

8. The chip for navigation of claim 7, wherein the processor is further configured to:

through the geomagnetic measurement, the rotation speed and the rotation difference value of the aircraft are estimated, the inertial information of the aircraft is obtained through the inertial sensor, and real-time ballistic measurement and ballistic prediction are carried out through the satellite and the nominal ballistic to determine the state quantity and the observed quantity of the Kalman filter;

and performing zero offset error correction on the output of the inertial sensor based on the state quantity and the observed quantity of the Kalman filter, and performing trajectory error correction on the real-time trajectory measurement and trajectory prediction of the satellite.

9. The chip for navigation of claim 7, wherein the processor is further configured to:

acquiring the maximum value, the minimum value and the average value of the geomagnetic signal of the previous window;

on the basis of the maximum value, the minimum value and the average value of the obtained geomagnetic signals of the previous window, normalization processing is carried out on the real-time geomagnetic signal points obtained in the current window one by one, and geomagnetic signal points after the current-time geomagnetic signal normalization are continuously obtained;

and analyzing whether zero points appear or not based on the geomagnetic signal points obtained after normalization, taking the acquired time values of two adjacent zero points as characteristic values of data analysis, calculating the periods of the sine waves in the time when the two current zero points appear based on the characteristic values, and calculating the rotating speeds of the aircraft in the two current times based on the periods of the sine waves in the two current times.

10. The chip for navigation of claim 7, wherein the processor is further configured to:

acquiring an original value of inertial navigation data within a sampling time Ts, and normalizing the original value of the inertial navigation data;

carrying out gravity elimination processing on the original value of the inertial navigation data after normalization processing;

calculating the period of a sine wave in the sampling time Ts based on the original value of the inertial navigation data subjected to gravity elimination processing, determining the time setting range of the next sampling time Ts based on the period of the sine wave in the sampling time, and taking the next sampling time as the sampling time Ts;

wherein the inertial navigation data includes a rotational speed of the aircraft acquired by the inertial sensor.