CN114111797A - Kalman filter, IP core and chip for navigation based on FPGA - Google Patents
Kalman filter, IP core and chip for navigation based on FPGA Download PDFInfo
- Publication number
- CN114111797A CN114111797A CN202111441052.9A CN202111441052A CN114111797A CN 114111797 A CN114111797 A CN 114111797A CN 202111441052 A CN202111441052 A CN 202111441052A CN 114111797 A CN114111797 A CN 114111797A
- Authority
- CN
- China
- Prior art keywords
- aircraft
- state
- matrix
- value
- time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 239000011159 matrix material Substances 0.000 claims abstract description 142
- 238000005259 measurement Methods 0.000 claims abstract description 69
- 238000005070 sampling Methods 0.000 claims description 57
- 238000012545 processing Methods 0.000 claims description 24
- 238000010606 normalization Methods 0.000 claims description 16
- 238000012937 correction Methods 0.000 claims description 14
- 230000005484 gravity Effects 0.000 claims description 7
- 230000008030 elimination Effects 0.000 claims description 6
- 238000003379 elimination reaction Methods 0.000 claims description 6
- 230000005389 magnetism Effects 0.000 claims description 6
- 238000007405 data analysis Methods 0.000 claims description 4
- 238000005516 engineering process Methods 0.000 abstract description 7
- 238000011161 development Methods 0.000 abstract description 3
- 238000004364 calculation method Methods 0.000 description 31
- 238000000034 method Methods 0.000 description 29
- 230000008569 process Effects 0.000 description 15
- 238000001914 filtration Methods 0.000 description 14
- 238000004422 calculation algorithm Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 8
- 230000002159 abnormal effect Effects 0.000 description 5
- 238000009499 grossing Methods 0.000 description 4
- 230000007704 transition Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000012546 transfer Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000005538 encapsulation Methods 0.000 description 2
- 238000012886 linear function Methods 0.000 description 2
- 230000026676 system process Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000002547 anomalous effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000005284 excitation Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000000977 initiatory effect Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000004806 packaging method and process Methods 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 238000005728 strengthening Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
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
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 datakEstimated value ZpreMultiplication 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):
Xk=f(Xk-1)+Wk-1 (1)
Zk-1=h(Xk-1)+Vk-1 (2)
in the above two formulae, XkIs the state vector of the system at time k, Zk-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; wk-1And Vk-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 QK-1And Rk-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-1And estimate the variance Pk-1Time, the non-linear function f (X)k-1) Combining a non-linear function f (X)k) In thatAfter the first-order Taylor expansion, the state equation can be simplified as follows:
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). WhereinThe 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. Pk-1Is the a posteriori estimated covariance at time k-1Covariance of (e) representing the uncertainty of the state.Representing the prior estimated covariance at time k, i.e.The covariance of (a). QkThen 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 thatAfter the first-order Taylor expansion, the measurement equation can be simplified as follows:
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. HkIs the transformation matrix of the state variables to measures (observations) at time k, representing the relationship of the state and observations connected. ZkIs a measurement (observation) value representing the filtered input. KkIs a state gain matrix, also known as kalman gain or kalman coefficient. RkThe 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:
X1,k+1=X2,ksinX1,k+0.1k+W1,k (12)
X2,k+1=X1,k+cos2X2,k-0.1k+W2,k (13)
the measurement model is as follows:
in the formula, X1,k+1The first variable, X, representing the state vector of the system at time k +12,kA second variable, X, representing the state vector of the system at time k1,kA first variable representing the state vector of the system at time k, k representing the time value, X2,k+1A second variable, Z, representing the state vector of the system at time k +11,k+1The first variable, Z, representing the measurement vector of the system at time k +12,k+1A second variable, W, representing the measurement vector of the system at time k +11,k、W2,k、V1,k+1、V2,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 valuekEstimated value ZpreMultiplication 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 errorThe accelerometer zero offset error delta a and the gyro zero offset error delta omega establish a state equation as follows:
wherein, Δ PkAnd Δ Pk+1Position errors at the time k and the time k +1 respectively; Δ VkAnd Δ Vk+1The speed errors at the time k and the time k +1 respectively;andattitude errors at the time k and the time k +1 respectively; Δ akAnd Δ ak+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 is1、B2The proportional coefficients of the accelerometer zero offset error and the gyroscope zero offset error; wk·a、Wk·ω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 requirementsIs 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 Xk+1=f[Xk,k]+WkThe form is as follows:
wherein F is a state translation matrix; i is3*3An identity matrix of 3x 3;is a coordinate transformation matrix.
Secondly, a measurement equation is established.
Establishing a measurement equation Zk+1=h[Xk+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 isk+1Measured for the time k + 1, h is the observation matrix, Xk+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) satisfyIs a zero mean white noise sequence; i is a 3x3 identity matrix.
Finally, an Extended Kalman Filter (EKF) is constructed.
Pk,k-1=fk,k-1Pk-1ff k,k-1+Qk-1
In the formula Pk,k-1Predicting covariance for the state; pkEstimating a covariance for the state; kkIs a Kalman gain matrix; qkA covariance matrix which is the system noise; rkIs 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:
Pk=[I-KkHk]Pk,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, quatkIs a quaternion at the moment K, K is the current calculation time, KkTo extend the Kalman gain, Pk.k-1A matrix is predicted for one step of the covariance of the system,for transposing the observation matrix, HkIn 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; epsilonkWhich 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, phik.k-1For the one-step prediction matrix of the state transition,is the mean value of the system noise, PkIs the system covariance matrix at time k, I is the identity matrix,covariance matrix, P, for system measurement noisek,k-1A matrix is predicted for one step of the covariance of the system,transpose matrix for one-step prediction of state transitions, Pk-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,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, Pk,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, dk=(1-b)/(1-bk+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 issRepresenting 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 QiTo normalize the processed data values, msIs 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.
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.
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 timeiAnd λ 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-rAe=F-(rA+rEF)e=(xF-(rA+rEF)cosλF,yF+(rA+rEF)sinλF)
wherein r isAIs 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, rEFDistance of expected track for satellite data detection, xFAnd yFIs the current X-axis and Y-axis coordinates, λ, of the aircraftFIs the angle of incidence.
Since the spiral trajectory passes the point of initiation (x)B,yB) Thus, the distance r of the desired track when the satellite data is detectedEFCan be expressed as:
wherein r isEFLength of expected flight path for satellite data detection, xFIs the current X-axis coordinate, y, of the aircraftFAs the Y-axis coordinate, x, of the aircraftBAs X-axis coordinate of the start-point, yBFor the Y-axis coordinate, λ, of the actuating pointFIs the angle of incidence, θn1To desired inclination, λMIs the terminal angle, K is the coefficient.
Thereafter, the reference line is determined:
wherein, bn2Is a polynomial constant term coefficient, xFAn X-axis coordinate, y, being the center of the reference circleFA Y-axis coordinate, x, being the center of the reference circleBIs the X-axis coordinate, y, of said intersectionBIs the Y-axis coordinate, λ, of said intersection pointFK is a constant coefficient for the angle of incidence.
In the determination of rEFAnd bn2Then, 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, yn2To output, bn2Is 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 defined0The 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 w0In the time range of
t0=2(t0max-t0min)
Wherein t is0maxIs the point in time at which the first maximum of the data occurs, t0minThe 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 provided0The normalized data curve in the time range at least comprises two zero points.
Then to the window w0And (5) normalizing the internal data.
At w0Determining maximum value w of geomagnetic signal in range0maxMinimum value w0minAnd the average value w0ave. The window w is obtained by calculation0And (5) normalizing the processed data to obtain:
wherein m is0Is 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, t0And t1Respectively 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 tnThe way of whether it is zero or not is: get tnMultiplying 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 isnIs zero. Through the window wiThe middle image passes through two adjacent zero points tinAnd ti(n+1)The elapsed time, the period of the sine wave at time n can be derived:
Tin=2(ti(n+1)-tin)
projectile rotational speed Rin:
By analogy, the current window w can be calculatediWithin a sine wave period T of each timeinAnd projectile rotational speed RinTherefore, 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 seti+1Is set to be lambda Tin(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 wi+1Is the current window wiBy a factor of lambda of the time of a single period of the last group of sinusoidal images, thereby ensuring that wi+1Has at least one sinusoidal period. Then to wi+1The data within the range is normalized, which yields:
in the formula: qi+1For after treatmentThe data value of (a); m isi+1Is wi+1The original value of the data within the range; w is aiave、wimaxAnd wiminAre respectively wiMean, maximum and minimum values of data within the range.
I.e. by wiCalculated last group period TinAnd a rotational speed RinTo set wi+1In the time range of, then wi+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 windowi+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 (10)
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 datakEstimated value ZpreMultiplication 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.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111441052.9A CN114111797B (en) | 2021-11-30 | 2021-11-30 | Kalman filter, IP core and navigation chip based on FPGA |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111441052.9A CN114111797B (en) | 2021-11-30 | 2021-11-30 | Kalman filter, IP core and navigation chip based on FPGA |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114111797A true CN114111797A (en) | 2022-03-01 |
CN114111797B CN114111797B (en) | 2024-02-20 |
Family
ID=80368314
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111441052.9A Active CN114111797B (en) | 2021-11-30 | 2021-11-30 | Kalman filter, IP core and navigation chip based on FPGA |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114111797B (en) |
Citations (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050251328A1 (en) * | 2004-04-05 | 2005-11-10 | Merwe Rudolph V D | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
CN102064799A (en) * | 2010-12-31 | 2011-05-18 | 南京理工大学 | Method for designing DCMFK (Debiased Converted Measurement Kalman filter) based on FPGA (Field Programmable Gate Array) |
CN107314718A (en) * | 2017-05-31 | 2017-11-03 | 中北大学 | High speed rotating missile Attitude estimation method based on magnetic survey rolling angular rate information |
CN107894235A (en) * | 2017-12-12 | 2018-04-10 | 中国人民解放军国防科技大学 | Model error compensation method for autonomous navigation system of ultra-high-speed aircraft |
CN108801242A (en) * | 2018-04-28 | 2018-11-13 | 沈阳理工大学 | A kind of combined type attitude measurement method under high dynamic environment |
CN109752749A (en) * | 2018-12-10 | 2019-05-14 | 北京航空航天大学 | A kind of high Attitude estimation method and system for revolving the low rotation component of aircraft |
US10323907B1 (en) * | 2016-08-26 | 2019-06-18 | Cummings Aerospace, Inc. | Proportional velocity-deficit guidance for ballistic targeting accuracy |
CN110672078A (en) * | 2019-10-12 | 2020-01-10 | 南京理工大学 | High spin projectile attitude estimation method based on geomagnetic information |
WO2020114301A1 (en) * | 2018-12-07 | 2020-06-11 | 惠州学院 | Magnetic-side roll angular velocity information-based rotary shell flight posture high-precision estimation method |
CN111486865A (en) * | 2019-01-29 | 2020-08-04 | 北京理工大学 | Transfer alignment filter, transfer alignment method and guided aircraft using transfer alignment filter |
CN111650958A (en) * | 2019-12-15 | 2020-09-11 | 湖北航天飞行器研究所 | Online path planning method for switching in route points of take-off section of fixed-wing unmanned aerial vehicle |
CN111692919A (en) * | 2020-01-16 | 2020-09-22 | 北京理工大学 | Precise guidance control method for aircraft with ultra-close range |
CN113011011A (en) * | 2021-03-02 | 2021-06-22 | 南京理工大学 | Shell track correction method and device, storage medium and electronic device |
CN113155156A (en) * | 2021-04-27 | 2021-07-23 | 北京信息科技大学 | Method and device for determining running information, storage medium and electronic device |
CN113432612A (en) * | 2021-06-21 | 2021-09-24 | 北京信息科技大学 | Navigation method, device and system of flying object |
CN113467245A (en) * | 2021-07-15 | 2021-10-01 | 北京信息科技大学 | Fractional order sliding mode control method, device and system of aircraft |
CN113587746A (en) * | 2021-10-08 | 2021-11-02 | 北京信息科技大学 | Method, device and system for measuring large-span rotating speed of projectile body based on geomagnetic information |
-
2021
- 2021-11-30 CN CN202111441052.9A patent/CN114111797B/en active Active
Patent Citations (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050251328A1 (en) * | 2004-04-05 | 2005-11-10 | Merwe Rudolph V D | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
CN102064799A (en) * | 2010-12-31 | 2011-05-18 | 南京理工大学 | Method for designing DCMFK (Debiased Converted Measurement Kalman filter) based on FPGA (Field Programmable Gate Array) |
US10323907B1 (en) * | 2016-08-26 | 2019-06-18 | Cummings Aerospace, Inc. | Proportional velocity-deficit guidance for ballistic targeting accuracy |
CN107314718A (en) * | 2017-05-31 | 2017-11-03 | 中北大学 | High speed rotating missile Attitude estimation method based on magnetic survey rolling angular rate information |
CN107894235A (en) * | 2017-12-12 | 2018-04-10 | 中国人民解放军国防科技大学 | Model error compensation method for autonomous navigation system of ultra-high-speed aircraft |
CN108801242A (en) * | 2018-04-28 | 2018-11-13 | 沈阳理工大学 | A kind of combined type attitude measurement method under high dynamic environment |
WO2020114301A1 (en) * | 2018-12-07 | 2020-06-11 | 惠州学院 | Magnetic-side roll angular velocity information-based rotary shell flight posture high-precision estimation method |
CN109752749A (en) * | 2018-12-10 | 2019-05-14 | 北京航空航天大学 | A kind of high Attitude estimation method and system for revolving the low rotation component of aircraft |
CN111486865A (en) * | 2019-01-29 | 2020-08-04 | 北京理工大学 | Transfer alignment filter, transfer alignment method and guided aircraft using transfer alignment filter |
CN110672078A (en) * | 2019-10-12 | 2020-01-10 | 南京理工大学 | High spin projectile attitude estimation method based on geomagnetic information |
CN111650958A (en) * | 2019-12-15 | 2020-09-11 | 湖北航天飞行器研究所 | Online path planning method for switching in route points of take-off section of fixed-wing unmanned aerial vehicle |
CN111692919A (en) * | 2020-01-16 | 2020-09-22 | 北京理工大学 | Precise guidance control method for aircraft with ultra-close range |
CN113011011A (en) * | 2021-03-02 | 2021-06-22 | 南京理工大学 | Shell track correction method and device, storage medium and electronic device |
CN113155156A (en) * | 2021-04-27 | 2021-07-23 | 北京信息科技大学 | Method and device for determining running information, storage medium and electronic device |
CN113432612A (en) * | 2021-06-21 | 2021-09-24 | 北京信息科技大学 | Navigation method, device and system of flying object |
CN113467245A (en) * | 2021-07-15 | 2021-10-01 | 北京信息科技大学 | Fractional order sliding mode control method, device and system of aircraft |
CN113587746A (en) * | 2021-10-08 | 2021-11-02 | 北京信息科技大学 | Method, device and system for measuring large-span rotating speed of projectile body based on geomagnetic information |
Non-Patent Citations (6)
Title |
---|
GIRBACIA 等: "Path and Trajectory Planning for Vehicles with Navigation Assistants", APPLIED MECHANICS AND MATERIALS, vol. 811, 1 November 2015 (2015-11-01), pages 300 - 304 * |
刘一康 等: "基于FPGA的卡尔曼滤波器设计与仿真", 第三十三届中国仿真大会论文集, pages 1 - 6 * |
尹思源: "用于电力巡线的自转旋翼无人机导航控制***的设计", 中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑, no. 2019, 15 February 2019 (2019-02-15), pages 031 - 313 * |
戴彬彬: "基于FPGA的高速低功耗卡尔曼滤波器的架构设计和实现", 中国优秀硕士学位论文全文数据库 信息科技辑, no. 2018, pages 9 - 10 * |
王军: "基于改进模糊PID的导弹飞行轨迹误差修正反馈控制", 智能计算机与应用, vol. 7, no. 3, pages 26 - 29 * |
谢峰: "地磁辅助的卫星/惯性组合导航技术研究", 中国优秀硕士学位论文全文数据库 信息科技辑, no. 2018, pages 136 - 407 * |
Also Published As
Publication number | Publication date |
---|---|
CN114111797B (en) | 2024-02-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108827310B (en) | Marine star sensor auxiliary gyroscope online calibration method | |
JP5243956B2 (en) | Self-calibration for inertial instrument based on real-time bias estimator | |
CN105737823B (en) | A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF | |
CN105806363B (en) | The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF | |
CN105571578B (en) | A kind of utilize what pseudo-observation replaced precise rotating platform to rotate in place modulation north finding method | |
CN105136145A (en) | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method | |
US20030028340A1 (en) | Hybrid inertial navigation method and device | |
CN112798021B (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
CN109000680A (en) | Gyroscope acceleration sensitive error coefficient acquisition methods, device and system | |
CN111238535A (en) | IMU error online calibration method based on factor graph | |
CN113074753A (en) | Star sensor and gyroscope combined attitude determination method, combined attitude determination system and application | |
CN106595669B (en) | Method for resolving attitude of rotating body | |
CN113984049B (en) | Method, device and system for estimating flight trajectory of aircraft | |
CN113566850B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
CN114111797B (en) | Kalman filter, IP core and navigation chip based on FPGA | |
CN110873577B (en) | Underwater rapid-acting base alignment method and device | |
Hajdu et al. | Complementary filter based sensor fusion on FPGA platforms | |
CN114897942B (en) | Point cloud map generation method and device and related storage medium | |
CN111207734A (en) | EKF-based unmanned aerial vehicle integrated navigation method | |
CN110954080A (en) | Magnetic compass calibration method for eliminating carrier magnetic interference | |
RU2326349C2 (en) | Inertial system | |
CN116105737A (en) | RISC-V-based pedestrian autonomous positioning method | |
CN114397480B (en) | Acoustic Doppler velocimeter error estimation method, device and system | |
CN114111840B (en) | DVL error parameter online calibration method based on integrated navigation | |
CN114019954B (en) | Course installation angle calibration method, device, computer equipment and storage medium |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |