CN114111797B - Kalman filter, IP core and navigation chip based on FPGA - Google Patents
Kalman filter, IP core and navigation chip based on FPGA Download PDFInfo
- Publication number
- CN114111797B CN114111797B CN202111441052.9A CN202111441052A CN114111797B CN 114111797 B CN114111797 B CN 114111797B CN 202111441052 A CN202111441052 A CN 202111441052A CN 114111797 B CN114111797 B CN 114111797B
- Authority
- CN
- China
- Prior art keywords
- aircraft
- data
- stage
- matrix
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000005259 measurement Methods 0.000 claims abstract description 67
- 238000005070 sampling Methods 0.000 claims description 58
- 238000004364 calculation method Methods 0.000 claims description 30
- 238000012545 processing Methods 0.000 claims description 24
- 238000010606 normalization Methods 0.000 claims description 23
- 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
- 238000007405 data analysis Methods 0.000 claims description 4
- 239000011159 matrix material Substances 0.000 abstract description 119
- 238000005516 engineering process Methods 0.000 abstract description 8
- 238000000034 method Methods 0.000 description 27
- 230000008569 process Effects 0.000 description 13
- 238000004422 calculation algorithm Methods 0.000 description 12
- 238000001914 filtration Methods 0.000 description 11
- 230000002159 abnormal effect Effects 0.000 description 7
- 238000012546 transfer Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 238000009499 grossing Methods 0.000 description 5
- 230000007704 transition Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 229920001971 elastomer Polymers 0.000 description 2
- 239000000806 elastomer Substances 0.000 description 2
- 230000026676 system process Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- PUAQLLVFLMYYJJ-UHFFFAOYSA-N 2-aminopropiophenone Chemical compound CC(N)C(=O)C1=CC=CC=C1 PUAQLLVFLMYYJJ-UHFFFAOYSA-N 0.000 description 1
- FVIGODVHAVLZOO-UHFFFAOYSA-N Dixanthogen Chemical compound CCOC(=S)SSC(=S)OCC FVIGODVHAVLZOO-UHFFFAOYSA-N 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005538 encapsulation Methods 0.000 description 1
- 230000005284 excitation Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000000691 measurement method 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
- 238000005096 rolling process Methods 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
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 navigation chip 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, a measurement truth, a jacobian matrix, and a measurement matrix based on the clock signal and the reset signal; and the output interface is configured to output the state prior estimated value, the state true value, the measurement true value, the jacobian matrix and the measurement matrix for state prediction and state update. The invention solves the technical problems of time consumption, 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 based on an FPGA, an IP core and a chip for navigation.
Background
The traditional modes for realizing Kalman filtering mainly comprise an ASIC (application specific integrated circuit), a DSP (digital signal processor) and the like, but the modes have certain defects and have the problem of slower speed.
In view of the above problems, no effective solution has been proposed at present.
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) to at least solve the technical problems of time consumption, higher cost and slower 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, a measurement truth, a jacobian matrix, and a measurement matrix based on the clock signal and the reset signal; and the output interface is configured to output the state prior estimated 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 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, higher cost and slower execution speed of the Kalman filter in the related technology are solved.
Drawings
FIG. 1 is a schematic diagram of a structure of an FPGA-based Kalman filter according to an embodiment of the invention;
FIG. 2 is a schematic 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 according to an embodiment of the 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 flowchart of an IP core encapsulation method of a kalman filter according to an embodiment of the invention.
FIG. 5 is a plot of normalized geomagnetic signals in accordance with an embodiment of the present invention;
fig. 6 is a sinusoidal graph of a normalized geomagnetic signal in accordance with an embodiment of the present invention.
Detailed Description
Example 1
According to an embodiment of the present invention, an FPGA-based kalman filter is provided, as shown in fig. 1, which includes 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 truth, a measurement truth, 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 estimate, the state truth, the measurement truth, the jacobian matrix, and the measurement matrix for state prediction and state update.
According to the Kalman filter based on the FPGA, through the structure, the technical problems that the Kalman filter in the related technology is time-consuming to develop, high in cost and low in execution speed are solved. In the embodiment, the Kalman filter realized based on the FPGA adopts a hardware parallel algorithm, and compared with the Kalman filter realized by other traditional processors, the Kalman filter has better expandability, faster speed and stronger instantaneity.
In an exemplary embodiment, the kalman filter further includes: the system 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 Kalman gain K and true value Z when updating the state optimal estimated value according to the input clock signal and the input data based on the control of the main module k Estimated value Z pre Multiplication of corresponding bits of (a).
And the matrix inversion module is configured to calculate matrix division operation in Kalman gain according to the clock signal, the reset signal and the matrix input based on the control of the main module so as to output an inverse matrix.
And the state updating module is configured to calculate a K moment jacobian matrix, a measurement matrix, a state prior estimated value, a measurement prior estimated value, a state true value and a measurement true value according to a clock signal, a reset signal, an input state estimated value and an input measurement true value based on the 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 a plurality of fixed-point multiplication modules, the advantages of expandability, instantaneity and the like of the FPGA are fully utilized, and the implementation method is quicker and more accurate than that of a traditional Kalman filter, and has the advantages of short development period, low cost and higher execution speed.
The embodiment of the application also provides an IP core comprising the FPGA-based Kalman filter. The designed Kalman filter is packaged into an IP core, and the IP core can be directly called when in use, so that a Kalman filtering algorithm can be realized more conveniently.
Example 2
There is also provided a chip for navigation according to an embodiment of the present invention, as shown in fig. 2, which 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 will not be described here.
A processor 24 configured to: measuring the rotating speed of the aircraft by using geomagnetism under the condition that the aircraft is in a first stage so as to estimate the flight track of the aircraft, wherein the first stage is a stage in which an inertial measurement device fails; starting the inertial sensor when the aircraft is in a second stage, and acquiring the working state of the inertial sensor based on the rotational speed of the aircraft measured by the geomagnetism and the rotational speed of the aircraft acquired by the inertial sensor to estimate the flight trajectory of the aircraft, wherein the second stage is a stage in which the rotational speed of the aircraft measured by the geomagnetism is lower than a set threshold value; performing initial alignment based on aircraft data acquired by satellites to estimate a flight trajectory of the aircraft when the aircraft is in a third stage, wherein the third stage is a stage in which the aircraft data can be captured by satellites, 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 information of the rotation speed, the attitude, the position and the speed of the aircraft measured by 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: estimating the rotating speed and the rotating difference value of the aircraft through geomagnetic measurement, acquiring inertial information of the aircraft through the inertial sensor, and carrying out real-time ballistic measurement and ballistic prediction through the satellite and the nominal trajectory so as to determine the state quantity and the observed quantity of the Kalman filter; and carrying out zero offset error correction on the output of the inertial sensor based on the state quantity and observed quantity of the Kalman filter, and carrying out 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: obtaining the maximum value, the minimum value and the average value of geomagnetic signals of a previous window; based on the maximum value, the minimum value and the average value of the geomagnetic signals of the last window, carrying out normalization processing on geomagnetic signal points acquired in the current window one by one, and continuously obtaining geomagnetic signal points after geomagnetic signal normalization at the current moment; based on the geomagnetic signal points obtained after normalization, analyzing whether zero points appear, taking the acquired time values of the occurrence of two adjacent zero points as characteristic values of data analysis, calculating the period of the sine wave in the time of the occurrence of the current two zero points based on the characteristic values, and calculating the rotating speed in the current two times of the aircraft based on the period of the sine wave in the current two times.
In an exemplary embodiment, the processor is further configured to: acquiring an original value of inertial navigation data in a sampling time Ts, and carrying out normalization processing on the original value of the inertial navigation data; carrying out gravity elimination treatment on the original value of the inertial navigation data after normalization treatment; calculating the period of the sine wave in the sampling time Ts based on the original value of the inertial navigation data after the 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; the inertial navigation data comprise the rotating speed of the aircraft acquired by the inertial sensor.
The navigation chip in the present embodiment can more accurately navigate an aircraft by the above-described structure.
Example 3
According to the embodiment of the invention, a Kalman filter based on the FPGA is also provided.
The essence of the kalman filter is a recursive predictive-correction method, which is limited to use in linear systems only. The Extended Kalman Filter (EKF) is one of the more effective processing methods to solve the nonlinear state estimation problem.
In order to increase the calculation speed, the present embodiment adopts the following method to design an extended kalman filter.
First, a nonlinear system considering white gaussian noise is introduced, and then 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 two formulas, X k Is the state vector of the system at time k, Z k-1 Is the measurement vector of the system at the time k-1; f (x) and h (x) in the formulas (1) and (2) represent a nonlinear state function and a measurement function of the system, respectively; w (W) k-1 And V k-1 Can be regarded as noise interference of the system during the process and the measurement, namely Gaussian white noise (White Gaussisn Noise) of a Kalman filtering algorithm, and the covariance is Q K-1 And R is k-1 。
In the kalman filtering method provided in this embodiment, two steps may be included:
(1) And (5) predicting. And predicting the state of the current k moment through the posterior estimation value of the previous moment, namely (k-1), so as to obtain an equation of the posterior estimation value of the current moment.
(2) Updating. As shown in fig. 3A, the estimated value of the prediction stage is corrected by measuring the value at the current time, and the posterior estimated value at the current time is obtained.
State estimation at known k-1 timeAnd estimated variance P k-1 At this time, a nonlinear function f (X k-1 ) The nonlinear function f (X k ) At->After the first-order Taylor expansion is carried out, the state equation can be simplified as follows:
Wherein F is k-1 Is after Taylor expansionIs a jacobian matrix at time k-1.
The time update equation of the kalman filter can be expressed by equations (4) and (5).
The time update equation, i.e. the prediction phase equation of the Kalman filtering algorithm, is a state variable prior estimated value and an error covariance prior estimated value of the current moment are calculated according to the state estimated value of the previous moment (k-1 moment). Wherein the method comprises the steps ofThe posterior state estimate, also called the optimal estimate, at time k-1 is the updated result. />The prior state estimation value at the k moment is the result of the k moment predicted according to the optimal estimation at the last moment (k-1 moment), and is the result of a prediction equation. P (P) k-1 Is the posterior estimated covariance (++) at time k-1>Covariance of) representing the uncertainty of the state. />Representing a priori estimated covariance at time k, i.e.>Is a covariance of (c). Q (Q) k The covariance of the excitation noise representing the process at time k, 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 called a prediction equation.
The nonlinear function h (X k-1 ) At the position ofAfter the first-order taylor expansion is performed, the measurement equation can be simplified as follows:
wherein H is k Is after Taylor expansion Is the measurement matrix at the moment k.
The measurement prediction error covariance matrix is:
the cross covariance matrix between the states and the measurements is:
the state update equations of the kalman filter can be represented by equations (9), (10), and (11).
Wherein 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 the state. H k Is the transformation matrix of state variables at time k into measurements (observations) representing the connected relationship of states and observations. Z is Z k Is a measured value (observed value) representing the filtered input. K (K) k Is a state gain matrix, also known as a kalman gain or kalman coefficient. R is R k The covariance of the measurement noise, i.e. the covariance of the system process, representing the k-time is used to represent the error between the state transition matrix and the actual process. The measurement update equation is also referred to as the correction equation.
The Kalman filter based on the FPGA 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: master module 14, matrix inversion module 322, fixed point multiplication module 324, state update module 326, and matrix multiplication module 328.
In this embodiment, an extended kalman filtering algorithm is implemented by the FPGA, and the operation is decomposed into four matrix operations of "add", "subtract", "multiply", "invert".
8 fixed-point multiplication modules 322 (qmult), a state update module 326, a matrix inversion module 322, and a sequential matrix multiplex module 328 are added to the main module 14. Wherein 7 CORDIC (trigonometric function calculation) modules 3261, 2 cordic_atan (inverse trigonometric function calculation) modules 3262, 11 qmult (fixed point multiplier of 18-bit integer, 30-bit decimal) modules 3263, 3 sqrt (open square calculation) modules 3264, 4 qdiv (divider calculation) modules 3265 and 4 rom_drive (register) modules 3266 are added in the state_update module; 4 qdiv modules and 2 called multiplier IP core mults are added in matrix inversion module 322; 4 coefficient_calc modules 3282 are added within the sequential_matrix_multiple module 328. Two fixed point number multiplier modules 32821 are provided in each vector multiplication module 3282, and one called multiplier IP core mult328211 is provided in each fixed point number multiplier.
In the state update module 326, the algorithm for calculating the jacobian matrix and the metrology matrix at time k using the modules is as follows:
let the discrete state model be:
X 1，k+1 ＝X 2，k sinX 1，k +0.1k+W 1，k (12)
X 2，k+1 ＝X 1，k +cos 2 X 2，k -0.1k+W 2，k (13)
the measurement model is as follows:
wherein X is 1，k+1 The first variable, X, representing the state vector of the system at time k+1 2，k A second variable representing the state vector of the system at time k，X 1，k The first variable representing the state vector of the system at time k, k representing the time value, X 2，k+1 The second variable, Z, representing the state vector of the system at time k+1 1，k+1 The first variable, Z, representing the system measurement vector at time k+1 2，k+1 A second variable representing the system measurement vector at time k+1, W 1，k 、W 2，k 、V 1，k+1 、V 2，k+1 The k moment 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 master module, the input parameters are: clk (clock signal), reset (reset signal); the output parameters are as follows: xhat1, xhat2 (state best estimate), xpre_sample1, xpre_sample2 (state prior estimate), 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 large modules, namely:
1. matrix_reverse matrix inversion module for calculating matrix division operation involved in Kalman gain, which calls 2 mult modules and 4 qdiv divider modules.
In the matrix_reverse matrix inversion module, the input parameters are: 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 of 18 bit integer, 30 bit decimal) modules for respectivelyThe Kalman gain K and the true value Z involved in calculating the optimal estimation value of the updated state k Estimated value Z pre Multiplication of corresponding bits of (a).
In 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 updating module is used for calculating state truth values Xr1 and Xr2, measurement truth values Zr1 and Zr2, state prior estimated values Xpre1 and Xpre2, measurement prior estimated values Zpre1 and Zpre2, jacobian matrix F and transpose f_trans-ose thereof, and measurement matrix H and transpose h_trans-ose thereof. At the same time, the rom module is called 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 module, the input parameters are: clk (clock signal), reset (reset signal), x1, x2, (input state estimation value, input xhat1 and xhat2 when calling), xc1, xc2 (input measurement true value, input xr_sample and xr_sample2 when calling); output parameters: xe1, xe2 (state prior estimate), ze1, ze2 (measurement prior estimate), xr1, xr2 (state true value), zr1, zr2 (measurement true value), F, f_transfer (jacobian F and transpose thereof), H, h_transfer (measurement matrix H and transpose thereof).
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 the power Q of 2 divided by 1.647), angle_radius (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 (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_valid (enable signal), data_in (data input), output parameters are: o_valid (output enable), data_out (output data), data_domain (remainder).
In qdiv (division calculation) module, the input parameters are: i_dividend_o (divisor of input 32 bits), i_divisor_o (divisor of input 32 bits), i_start (calculation start flag bit), i_clk (input clock); the output parameters are as follows: o_quoient (output 32-bit division result), o_complete (completion flag), o_overflow (overflow flag)
In the rom_drive (register) module, the input parameters are: clk_100M (clock), rst_n (reset), en (enable), and ROM_q (output).
4. 4 coefficient_calc modules (for 2X2 matrix operations) are added in the sequential_matrix_multiple module. Wherein there are two qmult modules within each vector multiplication computation module.
In the sequential_matrix_multiple 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 1 is computer_F_x_P_pre, representing the product of the covariance matrix and the jacobian matrix; the state 2 is computer_F_x_P_pre_x_F_transfer, representing the product of the last state result calculated and the jacobian transpose matrix; the state 3 is computer_P_pre_x_H_transfer, representing the product of the calculated a priori error and the H transpose matrix; the state 4 is computer_H_x_P_pre_x_H_transmit, representing the result of the last result multiplied by the measurement matrix; the state 5 is computer_K, representing the calculated Kalman gain; the state 6 is computer_K_x_H, representing the product of the calculated Kalman gain and the measurement transpose matrix; the state 7 is computer_P, representing the calculated error covariance, i.e., the updated value of the error covariance; the state of state 8 is state_wait_K, representing state wait; the state 9 is state_preparation_k, and represents that the state machine enters a 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, the Vivado software is used to encapsulate the IP core, and the specific flow is shown in fig. 4, and includes the following steps:
step S402, modifying the project. Step S404, whether the engineering function simulation or the time sequence exhibition is completely correct. If the engineering function simulation or the time sequence simulation is completely correct, the step S406 is executed, otherwise, the step S402 is skipped. Step S406, creating a blank IP. Step S408, selecting a storage location. Step S410, encapsulating the IP. The IP core of this project is packaged.
Step S412, judging whether the IP core is packaged successfully.
If an error occurs in the process of emulating or encapsulating the IP core, the engineering needs to be rechecked and modified, i.e. the step jumps back to step S402. After the IP core is packaged successfully, only one Block Design file is needed to be created in the Vivado software, and the packaged IP core can be added in the engineering. And reconfiguring pins and adding constraint files to complete the call of the IP core.
In this 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 filter algorithm can be more conveniently implemented.
Example 5
According to an embodiment of the application, a chip for navigation is provided, and the chip comprises a processor, wherein the processing comprises 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 a rotational speed of the aircraft with geomagnetism to estimate a flight trajectory of the aircraft in a case where the aircraft is in a first stage, wherein the first stage is a stage in which an inertial measurement device fails; a second stage module configured to start the inertial sensor and acquire an operating state of the inertial sensor based on a rotational speed of the aircraft measured by the geomagnetism and the rotational speed of the aircraft acquired by the inertial sensor to estimate a flight trajectory of the aircraft, wherein the second stage is a stage in which the rotational speed of the aircraft measured by the geomagnetism is below a set threshold; the third stage module is configured to perform initial alignment based on satellite acquired aircraft data to estimate a flight trajectory of the aircraft with the aircraft 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 is able to be captured by satellites. In an exemplary embodiment, the initial alignment may be performed by motion information of the aircraft, satellite velocity resolution angle information, nominal trajectory information, inertial information acquired by the satellites. The fourth stage module is configured to construct a kalman filter based on rotational speed, attitude, position and speed information of the aircraft measured by geomagnetism when the aircraft is in a fourth stage, and estimate a flight trajectory of the aircraft by using the kalman filter, 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 carrying out normalization processing on 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 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 an axial gyroscope working state of the flying machine based on the rotation speed of the aircraft measured by geomagnetism and the rotation speed of the aircraft detected by the gyroscope; judging the working state of the accelerometer through ballistic parameters; wherein the inertial sensor includes the gyroscope and the accelerometer.
In one exemplary embodiment, the second stage module is further configured to: acquiring an original value of inertial navigation data in the sampling time Ts, and carrying out normalization processing on the original value of the inertial navigation data; carrying out gravity elimination treatment on the original value of the inertial navigation data after normalization treatment; and calculating the period of the sine wave in the sampling time Ts based on the original value of the inertial navigation data after the 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 an exemplary embodiment, the fourth stage module is further configured to: estimating the rotating speed and the rotating difference value of the aircraft through geomagnetic measurement, acquiring inertial information of the aircraft through the inertial sensor, and carrying out real-time ballistic measurement and ballistic prediction through the satellite and the nominal trajectory so as to determine the state quantity and the observed quantity of the Kalman filter; and carrying out zero offset error correction on the output of the inertial sensor based on the state quantity and observed quantity of the Kalman filter, and carrying out trajectory error correction on the real-time trajectory measurement and trajectory prediction of the satellite. In one exemplary embodiment, the state quantities include speed error, position error, attitude error, and gyro zero offset; the observed quantity comprises position, speed, pitching, yawing and flying axial gyro zero offset.
The estimation device in this embodiment can implement the estimation method in the foregoing embodiment, and will not be described herein. The structure of the kalman filter in this embodiment is the same as that of the kalman filter in the above embodiment, and will not be described here again.
In the embodiment, when the rotating speed is lower than the inertial navigation starting threshold, 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 is invalid due to a high dynamic environment is effectively avoided. And the accuracy and the speed of initial alignment in the system are improved by combining satellite positioning data and bound trajectory parameters.
Example 6
According to an embodiment of the present invention, there is also provided a chip for navigation including a processor and a kalman filter. Wherein the processor is capable of performing the method of:
1. data is collected.
Relevant data is collected from scalar geomagnetism in a first stage, from gyroscopes, accelerometers in a second stage, from nominal trajectory and satellites in a third stage for corresponding navigation.
2. And constructing a Kalman function, correcting errors and resolving gestures.
After entering the fourth stage, entering the integrated navigation stage.
1) A kalman function is constructed.
Firstly, selecting the error amount of a system as a state variable: position error ΔP, velocity error Δv, attitude errorThe accelerometer zero offset error delta a and the gyro zero offset error delta omega are established as follows:
wherein DeltaP k And DeltaP k+1 The position errors of the moment k and the moment k+1 are respectively; deltaV k And DeltaV k+1 The speed errors at the time k and the time k+1 are respectively;and->The attitude errors at the k moment and the k+1 moment are respectively; Δa k And Deltaa k+1 Zero offset errors of the accelerometer are respectively the k moment and the k+1 moment; Δω k And Δω k+1 Gyro zero offset errors at the moment k and the moment k+1 respectively; b (B) 1 、B 2 The proportion coefficient of the zero offset error of the accelerometer and the zero offset error of the gyroscope; w (W) k·a 、W k·ω Random system dynamic noise of accelerometer and gyro error at k moment respectively, and average value thereofVariance satisfies->A zero-mean white noise sequence; k represents the moment, Δt is the time, +.>For the change matrix, n represents the navigation coordinate system and b represents the carrier coordinate system. St is an antisymmetric matrix.
State equation X, in which the above equation is written as a standard k+1 ＝f[X k ，k]+W k The form is as follows:
wherein F is a state translation matrix; i 3*3 A 3x3 identity matrix;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 measurement is the position and speed of the aircraft, and the specific formula is as follows:
wherein Z is k+1 For the measurement of the moment k+1, h is the observation matrix, X k+1 The state vector of the system at the time k+1, k being the current calculation time. The system noise is measured in the formula (4), and the mean and variance satisfyA zero-mean white noise sequence; iIs a 3x3 identity matrix.
Finally, an extended kalman filter algorithm (extended kalman filter, EKF) is constructed.
P k，k-1 ＝f k，k-1 P k-1 f f k，k-1 +Q k-1
P in the formula k，k-1 Predicting covariance for the state; p (P) k Estimating covariance for the state; k (K) k Is a Kalman gain matrix; q (Q) k A covariance matrix of system noise; r is R k Is the covariance matrix of the measured noise. The first two formulas are state prediction portions, and the last three formulas are update portions.
2) And carrying out gesture calculation.
The acceleration and gyroscope information (inertial navigation data) acquired from the sensor are used 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 rolling angle, pitch angle and yaw angle, and are calculated by adopting a formula (11); acc_t is inertial navigation data after eliminating gravity; rb2t is the Euler angle converted from the direction cosine matrix; ts is the sampling time.
3) And performing error correction.
And correcting the inertial navigation data by using satellite data and geomagnetic signals.
Calculating the error amount:
K＝(P*H′)/(H*P*H′+R)
z＝-x_h(1：6，k)
dx＝K*z
wherein z is the 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 transpose matrix of the observation matrix, R is measurement noise covariance, and x_h is a navigation information matrix calculated by an inertial navigation system;
correcting navigation data:
P k ＝[I-K k H k ]P k，k-1
wherein x_h is a navigation information matrix calculated by the inertial navigation system, dx is the obtained disturbance, R32 is R matrix 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 a quaternion turn direction cosine matrix, quat k The quaternion at the moment K, K is the current calculation moment, K k To expand the Kalman gain, P k.k-1 Is a one-step prediction matrix of the system covariance,for transpose of observation matrix, H k For observing matrix +.> The covariance matrix of the measurement noise at the moment k and the average value of the measurement noise are respectively obtained; epsilon k As residual error->For one-step prediction of system state quantity, +.>For the system state quantity at time k>For one-step prediction of system state quantity, +.>Phi is the system state quantity at the last moment k.k-1 For a one-step prediction matrix of state transitions, +.>Is of systematic noiseMean value, P k The system covariance matrix at k time is represented by I, which is an identity matrix,>covariance matrix P of system measurement noise k，k-1 One-step prediction matrix for system covariance, < +.>Transpose matrix, P, for one-step prediction of state transitions k-1 As the system covariance matrix of the last moment, Γ k-1 Driving the array for system noise +.>For the system noise covariance matrix of the last moment, < +.>The transposed matrix of the system noise driving matrix is represented by T, which is time.
Wherein,covariance matrix epsilon of measurement noise at k-1 moment k As residual error->Is the transposed matrix of the residual, T is the transposed symbol of the matrix, hk is the observation matrix, and P k，k-1 One-step prediction matrix for system covariance, < +. >Is the transposed matrix of the observation matrix +.>Is the average value of the measurement noise at the time of k-1, d k ＝(1-b)/(1-b k+1 ) (0 < b < 1) is a forgetting factor which acts to enhance the filtering effect on new data.
3. And carrying out Kalman updating.
Updating Kalman gain K:
K＝(P*H′)/（H*P*H′+R)
updating a filter state covariance matrix P:
P＝F*P*F′+G*Q*G′
P＝(P+P′)/2
cov(：，k)＝diag(P)
wherein P represents the filtered state covariance matrix, F represents the state transfer matrix, F ' represents the transpose of the state transfer matrix, G represents the process noise gain matrix, Q represents, G ' represents the transpose of the process noise gain matrix, P ' represents the transpose of the filtered state covariance matrix, cov (: k) represents the k-time data on the diagonal of the state covariance matrix.
Updating a state transfer matrix F:
where E represents a unit matrix of 15x15, and Rb2t represents a quaternion turn direction cosine matrix in the case where the quaternion is not 0.
The navigation chip in the embodiment utilizes geomagnetic signals and satellite data to carry out error correction on inertial navigation data and carries out combined navigation in stages, thereby solving the problem of inaccurate navigation in the related technology and having the beneficial effect of improving the navigation accuracy.
Example 7
According to an embodiment of the present invention, there is provided a chip for navigation including a processor and a kalman filter. The processor in this embodiment is different from the above embodiment in that normalization processing and smoothing processing are performed on data acquired by the inertial sensor, for example, acceleration data, and other processing steps are the same as those in the above embodiment, and are not repeated 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) The 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 200ms.
2) And obtaining the maximum value, the minimum value and the average value of the original value of the inertial navigation data in the current sampling time.
3) And removing abnormal data in the original value.
4) And smoothing the original value of the processed inertial navigation data.
In one embodiment, after the abnormal data is removed, the original value data thereof may be smoothed, for example: taking an average every n points, i.e
Wherein m is s Representing the original value of the inertial sensor after the smoothing treatment, thereby enabling the original value of the inertial sensor to achieve the aim of convenient treatment.
In other embodiments, this step may be omitted.
5) And normalizing the original value of the inertial navigation data after the smoothing treatment.
Normalization processing is performed based on the following formula:
wherein Q is i For normalizing the processed data value, m s For the smoothed original value, w, of the inertial sensor in the current sampling time range (i)ave 、w (i)max And w (i)min The average value, the maximum value and the minimum value of the original value of the inertial sensor in the sampling time range are respectively.
In one exemplary embodiment, w (i)ave The calculation process of (1) is as follows:
in order to make the calculation result more approximate to the true value, n represents the number of the original value points of the inertial sensor in the range of two adjacent maximum values 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 values of the abnormal data.
6) And calculating the rotating speed in the current sampling time.
The period of the sine wave in the sampling time can be obtained through the times x of zero crossing of the sine wave image in the sampling time:
where t represents a sampling time value.
Further calculating the rotational speed of the aircraft:
7) Navigation is performed based on the calculated rotational speed.
Compared with other existing navigation technologies, the technology provided by the embodiment uses the processed inertial navigation data to navigate, so that the interference factors in the measurement process are fewer, and the navigation is more reliable, stable and accurate.
Example 8
According to an embodiment of the present invention, there is provided a chip for navigation including a processor and a kalman filter. This embodiment differs 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 methods to process inertial navigation data (e.g., accelerometer data).
1) The 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 obtaining the maximum value, the minimum value and the average value of the original value of the inertial navigation data in the current sampling time.
3) And removing abnormal data in the original value.
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) The sine period in the current sampling time is calculated, and the current rotational speed is calculated based on the sine period.
Steps 1) to 6) are the same as steps 1) to 6) in embodiment 6, and are not repeated here.
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 in a current sampling time i Where λ is a multiple, and may be set according to practical situations, so long as it is ensured that the current sampling time range includes at least one sinusoidal period.
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 the 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, a chip for navigation is also provided. The navigation method of the navigation chip in executing the first stage, the second stage and the third stage is basically the same as the method of the above embodiment, and is different in that in the fourth stage, the present embodiment performs navigation based on a preset desired track as well as navigation data after the navigation data is acquired in the fourth stage.
The processor of the navigation chip of the present embodiment is further configured to:
1) And constructing a combined navigation model. 2) An extended kalman filter algorithm is constructed. 3) And carrying out gesture calculation. And carrying out error correction and attitude calculation based on the constructed combined navigation model and the constructed extended Kalman filtering algorithm. Steps 1) to 3) are the same as the steps of constructing the integrated navigation model, constructing the extended kalman filter algorithm and the pose solving in embodiment 7, and will not be described here again.
4) The desired trajectory is calculated.
In this embodiment, when the desired flight trajectory is introduced and integrated navigation is performed, not only the corrected inertial navigation data is considered, but also navigation is performed based on the desired flight trajectory.
For example, in the event that the satellite data is not updated, using the navigation parameters to navigate the aircraft using the spiral as a desired flight trajectory in the event that the satellite data is not updated; under the condition that the satellite data is updated, taking a straight line which passes through an intersection point of an expected flight track and a reference circle and points to the center of the reference circle under the condition that the satellite data is not updated as a datum line, adopting a straight line with a focal point connecting line parallel to the datum line as the expected flight track, carrying out error correction on inertial navigation data of the inertial sensor based on a combined navigation mathematical model by using the satellite data, and carrying out inertial calculation by adopting 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 can be expressed as:
A＝E-r A e＝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 A Is the radius of the reference circle, E is the intersection point of the expected track and the reference circle when no satellite data is detected, E is the direction vector, r EF X is the distance of the desired trajectory when satellite data is detected F And y F For the current X-axis and Y-axis of the aircraftCoordinates lambda F Is the angle of incidence.
Due to the spiral trajectory passing the start control point (x B ，y B ) Thus the distance r of the desired trajectory when satellite data is detected EF Can be expressed as:
wherein r is EF To detect the length of the desired flight trajectory when satellite data is detected, x F For the current X-axis coordinate, y of the aircraft F Is the Y-axis coordinate, x of the aircraft B X-axis coordinate, y of start control point B Y-axis coordinate of start control point lambda F For incident angle, theta n1 To a desired inclination angle lambda M And K is a coefficient for the terminal angle.
Thereafter, the reference line is determined:
wherein b n2 Is a polynomial constant term coefficient, x F An X-axis coordinate, y which is the center of the reference circle F The Y-axis coordinate and x are the center of the reference circle B Is the X-axis coordinate, y of the intersection point B Lambda is the Y-axis coordinate of the intersection point F K is a constant coefficient for the angle of incidence.
At r is calculated EF And b n2 After that, it can be seen that the track is directed to the center A of the reference circle through the E point, and the slope is tan lambda F Thus, the expected flight trajectory when satellite data is detected can be expressed as:
wherein y is n2 B for output n2 Is a constant term coefficient.
5) Fitting the desired 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, a chip for navigation is also provided. The navigation chip in this embodiment is substantially similar in structure and function to the navigation chip in the above-described embodiments, except that the processor of the navigation chip is further configured to process the geomagnetic signal. In the present application, geomagnetic signals are also referred to as geomagnetic data or geomagnetic information, and refer to data collected by geomagnetic sensors.
The existing rotation speed measurement scheme calculated through geomagnetic signals belongs to a post-processing mode, real-time performance is not high, and the range (first window) and the proportionality coefficient of a drawing window are uncertain, so that the embodiment makes the following improvement on the rotation speed measurement method through geomagnetic signals.
First define a first time window w 0 Is not limited in terms of the range of (a). Finding a first maximum value and a first minimum value in data corresponding to the received geomagnetic signal, and defining a first window w 0 Is within the time range of
t 0 ＝2(t 0max -t 0min )
Wherein t is 0max For the point in time when the first maximum of the data occurs, t 0min Is the point in time at which the first minimum occurs. Since the rotation period of the aircraft during the flight gradually becomes larger, the aircraft is positioned in the window w 0 The normalized data curve in the time range at least comprises two zero points.
Then to window w 0 And (5) normalizing the internal data.
At w 0 Obtaining the maximum value w of geomagnetic signals in a range 0max Minimum value w 0min Average value w 0ave . By calculation to obtain window w 0 And (3) carrying out normalization processing on the data to obtain:
wherein m is 0 Is the original value of the geomagnetic signal. The normalized geomagnetic signal is shown in fig. 5.
As shown in FIG. 6, the normalized sine curve of geomagnetic signal is t 0 And t 1 The time when the sinusoid passes the feature point, respectively. 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. Considering that the sampled values are not necessarily sinusoidal eigenvalues, and that eigenvalues near zero crossings are more easily distinguished than eigenvalues near peaks, troughs. Therefore, a value near the extraction zero point is selected as a characteristic value of data analysis, so that errors caused by sampling are reduced, and the rotating speed of the aircraft is obtained more accurately.
Determination point t n The mode of whether the zero point is the following: taking t n Multiplying the data values of the sampling points of adjacent time intervals on the left side and the right side, and if the obtained result is a negative value, t n Is zero. Through window w i The middle image passes through two adjacent zero points t in And t i(n+1) The period of the sine wave at the time of n can be obtained from the elapsed time:
T in ＝2(t i(n+1) -t in )
elastomer rotation speed R in ：
And so on, the current window w can be calculated i The period T of the sine wave at each moment in time in And the elastomer rotation speed R in Therefore, the real-time requirement of measuring the rotating speed is met. Meanwhile, when the two zero points are too close to each other (for example, smaller than a first distance threshold value, the calculation is biased, and the group of zero points is deleted to achieve the purpose of deleting the abnormal value, the situation that the data waveform jitter appears near the zero points, so that the abnormal rotation speed value is calculated (the rotation speed interval is from minus 30 to plus 300 rotations under normal conditions) is considered.
The rotation speed of the projectile body during the flying process is gradually reduced or even reversed, so that the next window w is formed i+1 Is set to lambda T in (the value of λ can be determined according to practical application requirements, in this algorithm λ=3), λ is a scaling factor, i.e. the next window w i+1 The time setting range of (2) is the current window w i Lambda times the single cycle time of the last set of sinusoidal images in the inner, thereby ensuring w i+1 At least one sinusoidal cycle is included in the time frame. And then to w i+1 Normalization processing is carried out on the data in the range, so that the following results are obtained:
wherein: q (Q) i+1 Is the processed data value; m is m i+1 Is w i+1 Original values of the data in the range; w (w) iave 、w imax And w imin W is respectively i Average, maximum and minimum values of data in the range.
I.e. by w i The last set of calculated periods T in And rotational speed R in To set w i+1 Is further within w i+1 Normalizing the data by using the maximum value, the minimum value and the average value of the data in the last window in the set time range, and calculating the period T of each moment in the window (i+1)n And rotational speed R (i+1)n And w is set at the last set of cycles and rotational speed within the window i+2 And so on until all the data are calculated, and finally the rotation speed calculation is completed.
Example 11
According to the embodiment of the invention, a chip for navigation is also provided. The navigation chip in this embodiment is substantially similar in structure and function to the navigation chip in the above-described embodiments, except that the processor of the navigation chip is further configured to process the geomagnetic signal.
The navigation chip is further configured to acquire the maximum value, the minimum value and the average value of geomagnetic data of the last window; based on the maximum value, the minimum value and the average value of the acquired geomagnetic data of the previous window, carrying out 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 normalization processing is a zero point or not so as to find out two adjacent zero points in the current window; calculating the rotating speeds 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 discrete plurality of 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 points on the left side and the geomagnetic data value of the adjacent time interval sampling points on the right side to obtain a result value; and under the condition that the result value is smaller than zero, judging that the current geomagnetic data point after normalization processing is zero, otherwise, judging that the current geomagnetic data point is not zero. For example, the values of the two moments corresponding to the two adjacent zero points are used as characteristic values of data analysis; calculating the period of the sine wave corresponding to the two moments based on the characteristic value; the rotational speed of the flying body is calculated based on the period of the sine wave.
In the case where the current window is the 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; defining a range of the first time window based on a point in time at which the first maximum occurs and a point in time at which the first minimum occurs; calculating the maximum value, the minimum value and the average value of geomagnetic data in the first time window, and taking the calculated maximum value, the calculated minimum value and the calculated average value of geomagnetic data in the first time window as the maximum value, the calculated minimum value and the calculated average value of geomagnetic data in the last time window respectively.
The navigation chip is further configured to normalize current geomagnetic data points acquired in real time in the current window based on the acquired maximum value, minimum value and average value of geomagnetic data of the previous window, judge whether the current geomagnetic data points after normalization are zero points, and execute the steps in a circulating mode until two adjacent zero points in the current window are found out; 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, reserving the two adjacent zero points.
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 an integer multiple of the single cycle time of the last group of sinusoidal images of 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 track 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 beneficial effect of improving the navigation accuracy.
Claims (4)
1. A navigation chip, comprising:
a kalman filter based on FPGA;
a processor configured to:
measuring the rotational speed of the aircraft by geomagnetism under the condition that the aircraft is in a first stage to estimate the flight track of the aircraft, wherein the first stage is a stage in which an inertial sensor fails;
starting the inertial sensor when the aircraft is in a second stage, and acquiring the working state of the inertial sensor based on the rotational speed of the aircraft measured by the geomagnetism and the rotational speed of the aircraft acquired by the inertial sensor to estimate the flight trajectory of the aircraft, wherein the second stage is a stage in which the rotational speed of the aircraft measured by the geomagnetism is lower than a set threshold value;
performing initial alignment based on aircraft data acquired by satellites to estimate a flight trajectory of the aircraft when the aircraft is in a third stage, wherein the third stage is a stage in which the aircraft data can be captured by satellites, and the aircraft data comprises attitude, position and speed information of the aircraft;
estimating a flight trajectory of the aircraft by using the kalman filter based on a rotational speed of the aircraft measured by the geomagnetism and the attitude, position, and speed information of the aircraft captured by satellites when the aircraft is in a fourth stage, wherein the fourth stage is a stage after the aircraft reaches a ballistic vertex;
When the aircraft is in the fourth stage, in the case that the satellite-based acquired aircraft data is not updated, using a spiral line as a desired flight trajectory in the case that the satellite-based acquired aircraft data is not updated, and using navigation parameters to navigate the aircraft; in the case where the satellite-based acquired aircraft data is updated, taking a straight line passing through an intersection point of the expected flight trajectory in the case where the satellite-based acquired aircraft data is not updated and pointing to the center of the reference circle as a reference line, and obtaining the expected flight trajectory by the following formula:
wherein, F for incident angle, theta n1 To a desired inclination angle lambda M For the terminal angle, r EF In order to detect the length of the expected flight trajectory when satellite data is detected, bn2 is a constant term coefficient and is used for determining the datum line, then, based on the Kalman filter, error correction is carried out on inertial navigation data of the inertial sensor by using the aircraft data acquired based on the satellite, inertial calculation is carried out by adopting the corrected inertial navigation data, and navigation parameters of the aircraft are obtained so as to navigate the aircraft; wherein, the reference circle center is determined based on the following parameters: current X-axis and Y-axis coordinates of the aircraft An angle of incidence, a radius of the reference circle, a distance of the desired flight trajectory when satellite data is detected; wherein the navigation parameters include attitude, position, speed of the aircraft, and rotational speed of the aircraft based on measurements of the geomagnetic and inertial sensors.
2. The navigation chip of claim 1, wherein the processor is further configured to:
estimating the rotating speed and the rotating difference value of the aircraft through geomagnetic measurement, acquiring inertial information of the aircraft through the inertial sensor, and carrying out real-time ballistic measurement and ballistic prediction through the satellite and the nominal trajectory so as to determine the state quantity and the observed quantity of the Kalman filter;
and carrying out zero offset error correction on the output of the inertial sensor based on the state quantity and observed quantity of the Kalman filter, and carrying out trajectory error correction on the real-time trajectory measurement and trajectory prediction of the satellite.
3. The navigation chip of claim 2, wherein the processor is further configured to:
obtaining the maximum value, the minimum value and the average value of geomagnetic signals of a previous window;
based on the maximum value, the minimum value and the average value of the geomagnetic signals of the last window, carrying out normalization processing on the geomagnetic signal points acquired in the current window one by one, and continuously obtaining geomagnetic signal points after geomagnetic signal normalization at the current moment;
Based on the geomagnetic signal points obtained after normalization, analyzing whether zero points appear, taking the acquired time values of the occurrence of two adjacent zero points as characteristic values of data analysis, calculating the period of the sine wave in the time of the occurrence of the current two zero points based on the characteristic values, and calculating the rotating speed in the current two times of the aircraft based on the period of the sine wave in the current two times.
4. The navigation chip of claim 3, wherein the processor is further configured to:
acquiring an original value of inertial navigation data in a sampling time Ts, and carrying out normalization processing on the original value of the inertial navigation data;
carrying out gravity elimination treatment on the original value of the inertial navigation data after normalization treatment; calculating the period of the sine wave in the sampling time Ts based on the original value of the inertial navigation data after the 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;
the inertial navigation data comprise the rotating 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 CN114111797A (en) | 2022-03-01 |
| CN114111797B true 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 (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7289906B2 (en) * | 2004-04-05 | 2007-10-30 | Oregon Health & Science University | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
-
2021
- 2021-11-30 CN CN202111441052.9A patent/CN114111797B/en active Active
Patent Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| 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 (9)
| Title |
|---|
| Path and Trajectory Planning for Vehicles with Navigation Assistants;Girbacia 等;Applied Mechanics and Materials;20151101;第811卷;300-304 * |
| 刘一康 等.基于FPGA的卡尔曼滤波器设计与仿真.第三十三届中国仿真大会论文集.2021,1-6. * |
| 地磁辅助的卫星/惯性组合导航技术研究;谢峰;中国优秀硕士学位论文全文数据库 信息科技辑(2018年第08期);I136-407 * |
| 基于FPGA的卡尔曼滤波器设计与仿真;刘一康 等;第三十三届中国仿真大会论文集;1-6 * |
| 基于FPGA的高速低功耗卡尔曼滤波器的架构设计和实现;戴彬彬;中国优秀硕士学位论文全文数据库 信息科技辑(2018年第8期);正文第9-10、27-28页 * |
| 基于改进模糊PID的导弹飞行轨迹误差修正反馈控制;王军;智能计算机与应用;第7卷(第3期);26-29 * |
| 李翠锦 等.FPGA技术及应用.西南交通大学出版社,2017,第8-9页. * |
| 用于电力巡线的自转旋翼无人机导航控制系统的设计;尹思源;中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑;20190215(2019 年 第02期);C031-313 * |
| 钟胜 等.成像自动目标识别的并行化实现技术.华中科技大学出版社,2020,第4-7页. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114111797A (en) | 2022-03-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP1585939B1 (en) | Attitude change kalman filter measurement apparatus and method | |
| JP5243956B2 (en) | Self-calibration for inertial instrument based on real-time bias estimator | |
| CN110006427B (en) | BDS/INS tightly-combined navigation method in low-dynamic high-vibration environment | |
| Sabatelli et al. | A double stage Kalman filter for sensor fusion and orientation tracking in 9D IMU | |
| CN112798021B (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
| EP3189303B1 (en) | Pedestrian navigation devices and methods | |
| CN112781586A (en) | Pose data determination method and device, electronic equipment and vehicle | |
| Poshtan et al. | Cascaded Kalman and particle filters for photogrammetry based gyroscope drift and robot attitude estimation | |
| CN113984049B (en) | Method, device and system for estimating flight trajectory of aircraft | |
| WO2020135183A1 (en) | Method and apparatus for constructing point cloud map, computer device, and storage medium | |
| CN113566850B (en) | Method and device for calibrating installation angle of inertial measurement unit and computer equipment | |
| CN110779514A (en) | Hierarchical Kalman fusion method and device for auxiliary attitude determination of bionic polarization navigation | |
| CN114111797B (en) | Kalman filter, IP core and navigation chip based on FPGA | |
| CN113074753A (en) | Star sensor and gyroscope combined attitude determination method, combined attitude determination system and application | |
| Hajdu et al. | Complementary filter based sensor fusion on FPGA platforms | |
| CN116105737A (en) | RISC-V-based pedestrian autonomous positioning method | |
| WO2022179602A1 (en) | Navigation information processing method and apparatus, electronic device, and storage medium | |
| CN114001730B (en) | Fusion positioning method, fusion positioning device, computer equipment and storage medium | |
| CN114019954B (en) | Course installation angle calibration method, device, computer equipment and storage medium | |
| CN115727871A (en) | Track quality detection method and device, electronic equipment and storage medium | |
| CN114897942A (en) | Point cloud map generation method and device and related storage medium | |
| CN114323007A (en) | Carrier motion state estimation method and device | |
| CN114111773B (en) | Combined navigation method, device, system and storage medium | |
| CN115839726B (en) | Method, system and medium for jointly calibrating magnetic sensor and angular velocity sensor | |
| CN114396936B (en) | Polynomial optimization-based inertial and magnetic sensor attitude estimation method and system |
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 |