CN115033844A - Unmanned aerial vehicle state estimation method, system and device and readable storage medium - Google Patents

Unmanned aerial vehicle state estimation method, system and device and readable storage medium Download PDF

Info

Publication number
CN115033844A
CN115033844A CN202210519072.1A CN202210519072A CN115033844A CN 115033844 A CN115033844 A CN 115033844A CN 202210519072 A CN202210519072 A CN 202210519072A CN 115033844 A CN115033844 A CN 115033844A
Authority
CN
China
Prior art keywords
estimation
value
state
extended kalman
matrix
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.)
Pending
Application number
CN202210519072.1A
Other languages
Chinese (zh)
Inventor
黄桃丽
辛艳峰
李琳琳
候洁明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hefei Siwill Intelligent Co ltd
Original Assignee
Hefei Siwill Intelligent Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hefei Siwill Intelligent Co ltd filed Critical Hefei Siwill Intelligent Co ltd
Priority to CN202210519072.1A priority Critical patent/CN115033844A/en
Publication of CN115033844A publication Critical patent/CN115033844A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/17—Function evaluation by approximation methods, e.g. inter- or extrapolation, smoothing, least mean square method
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64F—GROUND OR AIRCRAFT-CARRIER-DECK INSTALLATIONS SPECIALLY ADAPTED FOR USE IN CONNECTION WITH AIRCRAFT; DESIGNING, MANUFACTURING, ASSEMBLING, CLEANING, MAINTAINING OR REPAIRING AIRCRAFT, NOT OTHERWISE PROVIDED FOR; HANDLING, TRANSPORTING, TESTING OR INSPECTING AIRCRAFT COMPONENTS, NOT OTHERWISE PROVIDED FOR
    • B64F5/00—Designing, manufacturing, assembling, cleaning, maintaining or repairing aircraft, not otherwise provided for; Handling, transporting, testing or inspecting aircraft components, not otherwise provided for
    • B64F5/60—Testing or inspecting aircraft components or systems
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Algebra (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Manufacturing & Machinery (AREA)
  • Transportation (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)

Abstract

The invention provides an unmanned aerial vehicle state estimation method, which comprises the following steps: establishing a system model; according to the system model, at a specified moment, predicting the state estimation of the standard extended Kalman and initializing the iterative extended Kalman filtering estimation into the standard extended Kalman filtering estimation; taylor expansion at the estimation point of the standard extended Kalman filter estimation is performed, and iterative update calculation of the measurement is performed for a specified number of times. The invention improves the stability and reliability of the flight state of the unmanned aerial vehicle and further promotes the development and popularization of the unmanned aerial vehicle industry.

Description

Unmanned aerial vehicle state estimation method, system, equipment and readable storage medium
Technical Field
The invention belongs to the technical field of unmanned aerial vehicles, and particularly relates to an unmanned aerial vehicle state estimation method, system, equipment and a readable storage medium.
Background
Unmanned aerial vehicles refer to unmanned aerial vehicles or aircraft, and various types of unmanned aerial vehicles play a role in different fields. The state estimation is a core means for realizing the autonomous navigation control of the unmanned aerial vehicle, means that navigation parameters such as the on-orbit position, the speed and the attitude of an aircraft are determined in real time, and is one of important development directions of the navigation control technology of the unmanned aerial vehicle. The traditional state estimation method mainly adopts a least square method, a Kalman filtering algorithm and the like. In order to improve the autonomous navigation control performance, a state estimation method is continuously developed, and the current mainstream algorithm is a standard extended kalman filtering algorithm.
At present, the standard extended kalman filtering algorithm is to perform taylor first-order expansion at a kalman estimation point, convert a nonlinear system into a linear system for state estimation, but introduce an error term in model conversion. Errors may not be considered in a general non-linear system, and in a highly non-linear system, the error term is critical to the accuracy and stability of the state estimation. The second-order extended Kalman can effectively reduce the influence of an error term, but a second-order partial differential term needs to be calculated, so that the calculation amount is large.
Therefore, an unmanned aerial vehicle state estimation scheme needs to be designed to solve the problem that an error item in an extended Kalman algorithm of an unmanned aerial vehicle affects the stability and accuracy of state estimation.
Disclosure of Invention
In order to solve the influence of an error item in an extended Kalman algorithm of the unmanned aerial vehicle on the stability and accuracy of state estimation, the unmanned aerial vehicle state estimation method based on the iterative extended Kalman filtering is provided based on the principle that the realization is easy, the stability is good, and the increase of calculated amount is controllable, so that the stability and reliability of the flight state of the unmanned aerial vehicle are improved, and the development and popularization of the unmanned aerial vehicle industry are further promoted.
The technical scheme of the invention is as follows:
the invention provides an unmanned aerial vehicle state estimation method, which comprises the following steps:
establishing a system model;
predicting the state estimation of the standard extended Kalman and initializing the iterative extended Kalman filtering estimation as the standard extended Kalman filtering estimation at a specified moment according to a system model;
taylor expansion at the estimation point of the standard extended Kalman filter estimation is performed, and iterative update calculation of the measurement is performed for a specified number of times.
Further, the air conditioner is provided with a fan,
the establishing of the system model comprises establishing a system state equation and a system measurement equation.
Further, the system state equation is as follows:
Figure BDA0003640960430000021
wherein q is k+1 Is a unit quaternion value at the moment of k + 1; q. q.s k Is a unit quaternion value at time k; delta theta mx Is the measured value of the angle variation of the X axis; delta theta my Measured value 2 delta theta of angle variation of Y axis mz A measurement value indicating a Z-axis angle change amount; delta theta bx A deviation value indicating an amount of angular change of the X-axis; delta theta by A deviation value indicating an amount of angular change of the Y-axis; delta theta bz A deviation value representing a Z-axis angle variation; delta theta nx A noise value representing an amount of change in the X-axis angle; delta theta ny A noise value representing a variation amount of the Y-axis angle; delta theta nz A noise value representing the amount of change in the Z-axis angle.
Further, the system measurement equation is as follows:
z mag ＝H Mag ·x+R Mag ；
wherein z is mag Acquiring data for acquisition of a magnetometer sensor; h Mag Measuring the Jacobian matrix value of an equation for the system; r Mag X represents the system state variables, which is the noise coefficient matrix of the magnetometer.
Further, the prediction formula of the state estimation of the standard extended kalman is as follows:
Figure BDA0003640960430000022
wherein F is the Jacobian matrix of the system state equation, T is the matrix transpose operator,
Figure BDA0003640960430000023
and P k+1|k Are all covariance at time k +1, P k|k Covariance value at time k, Q k+1|k The noise matrix at time k + 1.
Further, looping through a specified number of times, performing Taylor expansion at the estimated points of the standard extended Kalman filter estimation, comprising: the jacobian matrix of the system measurement equation and the transition matrix of the observed noise are executed.
Further, the jacobian matrix of the system measurement equation is as follows:
Figure BDA0003640960430000031
wherein i is an iteration control quantity, i is 1, …, and N; q. q.s 0 ，q 1 ，q 2 ，q 3 Is an attitude quaternion value, m N Magnetic field value m of north of navigation coordinate system E For navigation of magnetic field values of the east of the coordinate system, m D The magnetic field value of the navigation coordinate system earth, m is the magnetic field measurement value under the body coordinate system, m NED A magnetic field vector in the northeast direction under the navigation coordinate system, q is a quaternion vector,
Figure BDA0003640960430000032
jacobian matrix, m, for magnetic field measurements with respect to attitude quaternion b For measuring the deviation value of the magnetic field, R B←N A rotation matrix from the geodetic coordinate system to the body coordinate system.
Further, the transition matrix of the observed noise is as follows:
Figure BDA0003640960430000033
where v is the measurement noise.
Further, the calculation formula for the specified number of iterations of the measurement update includes:
Figure BDA0003640960430000034
Figure BDA0003640960430000035
Figure BDA0003640960430000036
x k+l|k+1，i+1 ＝x k+1|k +K k，i [z Mag -(h k (x k|k，i )+H k，i (x k|k-1 -x k|k，i ))]；
wherein S is an intermediate calculation variable, K k，i In order to be the value of the kalman gain,
Figure BDA0003640960430000037
covariance value of i +1 th iteration at time k +1, x k+1|k+1，i+1 Is the state value of the i +1 th iteration at time k +1, h k (x k|k，i )+H k，i (x k|k-1 -x k|k，i ) At x for a priori estimation k|k，i The value of (d); and I is an identity matrix.
Further, the method further comprises:
and after the last iteration updating calculation is executed, outputting a calculation result of the last iteration, and using the calculation result for initializing the state parameters and the covariance matrix at the next moment of the specified moment.
Further, after the last iteration update calculation is performed, the update formula of the state estimation and estimation error covariance is as follows:
x k|k ＝x k|k,N+1 ；
P k|k ＝P k|k,N+1 ；
wherein x is k|k For state estimation, P k|k To estimate the error covariance, k denotes the time of day and N is the maximum number of iterations.
In one aspect, the present invention further provides a system for estimating a state of an unmanned aerial vehicle, where the system includes:
the model building module is used for building a system model;
the prediction module is used for predicting the state estimation of the standard extended Kalman at a specified time according to the system model;
the initialization module is used for initializing iterative extended Kalman filtering estimation into standard extended Kalman filtering estimation at a specified time according to a system model;
and the iteration module is used for circularly appointing times, executing Taylor expansion at an estimation point estimated by the standard extended Kalman filtering, and carrying out iterative updating calculation of the appointed times of measurement.
Further, the system further comprises:
and the circulating module is used for outputting the last calculation result after the last iterative updating calculation is executed, and is used for initializing the state parameters and the covariance matrix at the next moment of the specified moment.
The invention also provides a device comprising a memory, a processor and a computer program stored on the memory and capable of running on the processor, the processor implementing the steps of the method described above when executing the computer program.
In another aspect, the present invention also provides a computer-readable storage medium, on which a computer program is stored, wherein the computer program, when executed by a processor, implements the steps of the method described above.
According to the unmanned aerial vehicle state estimation method and system, the Taylor series expansion is performed on the new estimation point again in an iteration mode to reduce the linear error, the high-order performance of the extended Kalman is realized, and the influence of a linear error term on the state estimation accuracy is reduced.
Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. The objectives and other advantages of the invention will be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to these drawings without creative efforts.
Fig. 1 shows a flow chart of a method for estimating the state of a drone according to an embodiment of the invention.
Fig. 2 shows a schematic structural diagram of a drone state estimation system according to an embodiment of the invention.
Fig. 3 shows a schematic diagram of an apparatus according to an embodiment of the invention.
Fig. 4 shows a schematic structural diagram of a readable storage medium according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Specifically, as shown in fig. 1, the present invention provides a method for estimating a state of an unmanned aerial vehicle, where the method includes:
establishing a system model;
according to the system model, at a specified moment, predicting the state estimation of the standard extended Kalman and initializing the iterative extended Kalman filtering estimation into the standard extended Kalman filtering estimation;
taylor expansion at the estimation point of the standard extended Kalman filter estimation is performed, and iterative update calculation of the measurement is performed for a specified number of times.
This will be explained in detail below.
1. Establishing a system model
In order to fully display a specific algorithm model of the iterative calculation formula, attitude estimation is adopted as a modeling object, and the problem of the attitude estimation is solved.
In this embodiment, the building of the system model includes building a system state equation and a system measurement equation, where:
the system state equation for the system is as follows:
Figure BDA0003640960430000061
wherein q is k+1 A unit quaternion value at time k + 1; q. q.s k Is a unit quaternion value at time k; delta theta mx Is the measured value of the angle variation of the X axis; delta theta my Is the measured value of the angle variation of the Y axis; delta theta mz A measurement value indicating a Z-axis angle change amount; delta theta bx A deviation value indicating an amount of angular change of the X axis; delta theta by A deviation value indicating an amount of angular change of the Y-axis; delta theta bz A deviation value representing a Z-axis angle variation; delta theta nx A noise value representing an amount of change in the X-axis angle; delta theta ny A noise value representing a variation of the Y-axis angle; delta theta nz A noise value representing a Z-axis angle variation.
The system measurement equation of the system is as follows:
z mag ＝H Mag ·x+R Mag ；
wherein z is mag Acquiring data for acquisition of a magnetometer sensor; h Mag Measuring the Jacobian matrix value of an equation for the system; r Mag The noise figure matrix of the magnetometer, x represents the system state variables (magnetic field values).
In addition, the noise of the gaussian system of the system model of the present embodiment conforms to the normal distribution, that is, the system model of the present embodiment has a constraint condition, and the constraint condition is determined by the following formula:
w k ～(0，Q k )
v k ～(0，R k )；
wherein w is process noise and v isMeasuring noise, wherein k is a designated time value; q k Is w k Variance matrix of R k Is v is k The variance matrix of (2).
In the system model, it is also necessary to initialize the filter, that is, it is necessary to assign initial values to the state parameters and covariance matrix in the system model:
Figure BDA0003640960430000071
Figure BDA0003640960430000072
wherein, the first and the second end of the pipe are connected with each other,
Figure BDA0003640960430000073
in order to assign the state parameter after the initial value,
Figure BDA0003640960430000074
to assign the initial covariance matrix, x 0 In order to be the initial state value,
Figure BDA0003640960430000075
for the initial state estimate, the covariance matrix is
Figure BDA0003640960430000076
And calculating the covariance of the corresponding quaternion according to the rotation vector variance 0.1.
2. Prediction of state estimation for standard extended kalman
In this embodiment, the prediction of state estimation of standard extended kalman may be performed according to a system model. The prediction formula of the state estimation of the standard extended Kalman is as follows:
Figure BDA0003640960430000077
wherein F is the Jacobian matrix of the system state equation, and T is the momentThe matrix transpose operator is used to perform the operations,
Figure BDA0003640960430000078
and P k+1k Are all covariance at time k +1, P k|k Covariance value of k time, Q k+1|k The noise matrix at time k + 1.
Q k+1k Can be determined by the following formula:
Q k+1|k ＝G·Q k|k ·G T ；
where G is the noise transfer matrix.
F can be determined by the following formula:
Figure BDA0003640960430000079
g can be determined by the following formula:
Figure BDA00036409604300000710
in conclusion, q can be obtained k+1|k ＝F·q k|k ·F T +G·Q k|k 。
Figure BDA0003640960430000081
Noise which is the amount of angular change at time k.
3. Initializing iterative extended Kalman filter estimation to standard extended Kalman filter estimation
And initializing the iterative extended Kalman filter estimation to be an output value of the standard extended Kalman filter estimation at the moment k, wherein the output value comprises initial iterative state parameters and a covariance matrix. And performing state estimation of iterative extended Kalman filtering through a measurement module.
In this embodiment:
Figure BDA0003640960430000082
is equivalent to q k+1|k，0 ＝q k+1|k ；
Figure BDA0003640960430000083
Corresponding to P k+1|k，0 ＝P k+1|k ；
Wherein q is k+1|k And P k+1|k And setting the output of each calculation of the standard extended Kalman filter as an initial value of an iteration part.
Figure BDA0003640960430000084
The updated state for the first iteration at time k,
Figure BDA0003640960430000085
is the covariance value of the first iteration at time k.
4. Performing Taylor expansion at the estimation points of a standard extended Kalman Filter estimation
Assuming i as an iteration control quantity, and a loop i is 1, …, N (where N is the measurement update expected iteration number), taylor expansion at a new estimation point (the estimation point of the standard extended kalman filter estimation) is performed, that is, the following equation is performed:
jacobian matrix (H) of the system measurement equation of the ith order k，i ) The following:
Figure BDA0003640960430000086
wherein q is 0 ，q 1 ，q 2 ，q 3 Is an attitude quaternion value, m N Magnetic field value, m, of the north of the navigational coordinate system E For navigation of magnetic field values of the east of the coordinate system, m D The magnetic field value of the navigation coordinate system earth is taken as m, the magnetic field measurement value under the body coordinate system is taken as m NED A magnetic field vector in the northeast direction under the navigation coordinate system, q is a quaternion vector,
Figure BDA0003640960430000087
jacobian matrix, R, for magnetic field measurements with respect to attitude quaternion B←N Is a rotation matrix from the geodetic coordinate system to the body coordinate system,m b the bias value is measured for the magnetic field.
Observing the transfer matrix (M) of the noise k，i ) The following were used:
Figure BDA0003640960430000088
5. iterative update calculation for a specified number of measurements
The measurement iterative update calculation formula comprises:
Figure BDA0003640960430000091
Figure BDA0003640960430000092
Figure BDA0003640960430000093
x k+1|k+1，i+1 ＝x k+1|k +H k，i [z Mag -(h k (x k|k，i )+H k，i (x k|k-1 -x k|k，i ))]；
wherein S is an intermediate calculation variable, K k，i In order to be the value of the kalman gain,
Figure BDA0003640960430000094
covariance value of i +1 th iteration at time k +1, x k+1|k+1，i+1 Is the state value of the i +1 th iteration at time k +1, h k (x k|k，i )+H k，i (x k|k-1 -x k|k，i ) At x for a priori estimation k|k，i The value of (d); and I is an identity matrix.
6. Status update output
After the last iterative update calculation is performed, the calculation result of the last iteration is output and used for initializing the state parameters and the covariance matrix at the next moment (the k +1 moment) of the specified moment.
Wherein, after the last iterative update calculation is executed, the last calculation result can be estimated by the following state (x) k|k ) And estimate error covariance (P) k|k ) Represents:
x k|k ＝x k|k，N+1 ；
P k|k ＝P k|k，N+1 。
in summary, the maximum iteration number N is an adjustable parameter, and is set according to the computing capability of the hardware platform, and the optimal iteration number of the selected self-driving platform is tested, so that the calculated amount is not out of bounds. Through selecting the controllable calculated quantity adjusting parameter (N) of the platform, the stability and the reliability of the flight control system of the self-driving platform of the unmanned aerial vehicle are improved.
As shown in fig. 2, this embodiment further provides an unmanned aerial vehicle state estimation system, where the system includes:
the model building module is used for building a system model;
the prediction module is used for predicting the state estimation of the standard extended Kalman at a specified time according to the system model;
the initialization module is used for initializing the iterative extended Kalman filtering estimation into standard extended Kalman filtering estimation at a specified time according to the system model;
and the iteration module is used for circularly appointing times, executing Taylor expansion at an estimation point estimated by the standard extended Kalman filtering, and carrying out iterative updating calculation of the appointed times of measurement.
The system further comprises:
and the circulation module is used for outputting a calculation result of the last time after the last iterative update calculation is executed, and is used for initializing the state parameters and the covariance matrix of the next moment of the specified time, wherein after the last iterative update calculation is executed, an update formula of the state estimation and the estimation error covariance is as follows:
x k|k ＝x k|k，N+1 ；
P k|k ＝P k|k，N+1 。
the functions and implementation manners realized by the steps of the unmanned aerial vehicle state estimation method are consistent with the other functions and implementation manners of the modules in the unmanned aerial vehicle state estimation system, and therefore, the description is omitted here.
In addition, the present invention also provides an apparatus (fig. 3) and a computer-readable storage medium (fig. 4), wherein:
the apparatus comprises a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the steps of the above method when executing the computer program.
The readable storage medium has stored thereon a computer program which, when being executed by a processor, carries out the steps of the above-mentioned method.
In addition, the method is mainly suitable for the unmanned aerial vehicle self-driving platform, and the unmanned aerial vehicle self-driving platform is required to be provided with at least one group of gyroscope and magnetometer sensors, wherein the gyroscope is used for acquiring angle variation and magnetometer sensors
Figure BDA0003640960430000101
Δ θ of (1) mx Term, magnetometer sensor is used for magnetic field data acquisition (i.e. acquiring z) mag )。
Although the present invention has been described with reference to a preferred embodiment, it should be understood that various changes, substitutions and alterations can be made herein without departing from the spirit and scope of the invention as defined by the appended claims.

Claims (15)

1. A method for estimating the state of an unmanned aerial vehicle, the method comprising:
establishing a system model;
predicting the state estimation of the standard extended Kalman and initializing the iterative extended Kalman filtering estimation as the standard extended Kalman filtering estimation at a specified moment according to a system model;
taylor expansion at the estimation point of the standard extended Kalman filter estimation is performed, and iterative update calculation of the measurement is performed for a specified number of times.
2. The method of claim 1,
the establishing of the system model comprises establishing a system state equation and a system measurement equation.
3. The method of claim 2, wherein the system state equation is as follows:
Figure FDA0003640960420000011
wherein q is k+1 Is a unit quaternion value at the moment of k + 1; q. q of k Is a unit quaternion value at time k; delta theta mx Is the measured value of the angle variation of the X axis; delta theta my Is the measured value of the angle variation of the Y axis; delta theta mz A measurement value indicating a Z-axis angle change amount; delta theta bx A deviation value indicating an amount of angular change of the X-axis; delta theta by A deviation value indicating an amount of angular change of the Y axis; delta theta bz A deviation value representing a Z-axis angle variation; delta theta nx A noise value representing an amount of change in the X-axis angle; delta theta ny A noise value representing a variation amount of the Y-axis angle; delta theta nz A noise value representing a Z-axis angle variation.
4. A method as claimed in claim 3, wherein the system measurement equation is as follows:
z mg ＝H Mag ·x+R Mag ；
wherein z is mag Acquiring data for the acquisition of magnetometer sensors; h Mag Measuring the Jacobian matrix value of an equation for the system; r Mag The noise figure matrix of the magnetometer, x represents the system state variables.
5. The unmanned aerial vehicle state estimation method of claim 4, wherein the prediction formula of the state estimation of the standard extended Kalman is as follows:
Figure FDA0003640960420000021
wherein F is the Jacobian matrix of the system state equation, T is the matrix transpose operator,
Figure FDA0003640960420000022
and P k+1|k Covariance of all k +1 instants, P k|k Covariance value at time k, Q k+1|k The noise matrix at time k + 1.
6. The unmanned aerial vehicle state estimation method of claim 5, wherein a Taylor expansion at an estimation point of a standard extended Kalman filter estimation is performed for a specified number of cycles, comprising: the jacobian matrix of the system measurement equation and the transition matrix of the observed noise are executed.
7. The method of claim 6, wherein the Jacobian matrix of the system measurement equations is as follows:
Figure FDA0003640960420000023
wherein i is an iteration control quantity, i is 1, …, and N; q. q.s 0 ，q 1 ，q 2 ，q 3 Is an attitude quaternion value, m N Magnetic field value m of north of navigation coordinate system E For navigation of magnetic field values of the east of the coordinate system, m D The magnetic field value of the navigation coordinate system earth, m is the magnetic field measurement value under the body coordinate system, m NED A magnetic field vector in the northeast direction under the navigation coordinate system, q is a quaternion vector,
Figure FDA0003640960420000024
jacobian matrix, m, for magnetic field measurements with respect to attitude quaternion b For measuring the deviation value of the magnetic field, R B←N A rotation matrix from the geodetic coordinate system to the body coordinate system.
8. The method of claim 7, wherein the transition matrix of observed noise is as follows:
Figure FDA0003640960420000025
where v is the measurement noise.
9. The method of claim 8, wherein iteratively updating the calculation formula for a specified number of measurements comprises:
Figure FDA0003640960420000026
Figure FDA0003640960420000027
Figure FDA0003640960420000028
x k+1|k+1，i+1 ＝x k+1|k +K k，i [z Mag -(h k (x k|k，i )+H k，i (x k|k-1 -x k|k，i ))]；
wherein S is an intermediate calculation variable, K k，i In order to be the value of the kalman gain,
Figure FDA0003640960420000031
covariance value of i +1 th iteration at time k +1, x k+1|k+1，i+1 Is the state value of the i +1 th iteration at time k +1, h k (x k|k，i )+H k，i (x k|k-1 -x k|k，i ) For a priori estimation at x k|k，i The value of (d); and I is an identity matrix.
10. A method of estimating the state of a drone according to any of claims 1-9, characterized in that the method further comprises:
and after the last iteration updating calculation is executed, outputting a calculation result of the last iteration, and using the calculation result for initializing the state parameters and the covariance matrix at the next moment of the specified moment.
11. The method of claim 10, wherein after performing the last iterative update calculation, the update formula of the covariance of the state estimation and estimation error is as follows:
x k|k ＝x k|k，N+1 ；
P k|k ＝P k|k，N+1 ；
wherein x is k|k For state estimation, P k|k To estimate the error covariance, k denotes the time of day and N is the maximum number of iterations.
12. An unmanned aerial vehicle state estimation system, the system comprising:
the model building module is used for building a system model;
the prediction module is used for predicting the state estimation of the standard extended Kalman at a specified time according to the system model;
the initialization module is used for initializing the iterative extended Kalman filtering estimation into standard extended Kalman filtering estimation at a specified time according to the system model;
and the iteration module is used for circularly appointing times, executing Taylor expansion at an estimation point estimated by the standard extended Kalman filtering, and carrying out iterative updating calculation of the appointed times of measurement.
13. The drone state estimation system of claim 12, further comprising:
and the circulating module is used for outputting the last calculation result after the last iterative updating calculation is executed, and is used for initializing the state parameters and the covariance matrix at the next moment of the specified moment.
14. An apparatus comprising a memory, a processor and a computer program stored on the memory and capable of running on the processor, characterized in that the steps of the method of any of claims 1-11 are implemented when the computer program is executed by the processor.
15. A computer-readable storage medium, on which a computer program is stored which, when being executed by a processor, carries out the steps of the method according to any one of claims 1 to 11.
CN202210519072.1A 2022-05-12 2022-05-12 Unmanned aerial vehicle state estimation method, system and device and readable storage medium Pending CN115033844A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210519072.1A CN115033844A (en) 2022-05-12 2022-05-12 Unmanned aerial vehicle state estimation method, system and device and readable storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210519072.1A CN115033844A (en) 2022-05-12 2022-05-12 Unmanned aerial vehicle state estimation method, system and device and readable storage medium

Publications (1)

Publication Number Publication Date
CN115033844A true CN115033844A (en) 2022-09-09

Family

ID=83120260

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210519072.1A Pending CN115033844A (en) 2022-05-12 2022-05-12 Unmanned aerial vehicle state estimation method, system and device and readable storage medium

Country Status (1)

Country Link
CN (1) CN115033844A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116192571A (en) * 2023-02-06 2023-05-30 中国人民解放军火箭军工程大学 Unmanned aerial vehicle ISAC channel estimation method under beam dithering effect
CN117268409A (en) * 2023-08-30 2023-12-22 苏州大成运和智能科技有限公司 Unmanned vehicle local positioning method based on iterative extended Kalman filtering

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020087845A1 (en) * 2018-10-30 2020-05-07 东南大学 Initial alignment method for sins based on gpr and improved srckf
CN111189442A (en) * 2020-01-11 2020-05-22 郑州轻工业大学 Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
CN112346479A (en) * 2020-11-18 2021-02-09 大连海事大学 Unmanned aircraft state estimation method based on centralized Kalman filtering

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2020087845A1 (en) * 2018-10-30 2020-05-07 东南大学 Initial alignment method for sins based on gpr and improved srckf
CN111189442A (en) * 2020-01-11 2020-05-22 郑州轻工业大学 Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
CN112346479A (en) * 2020-11-18 2021-02-09 大连海事大学 Unmanned aircraft state estimation method based on centralized Kalman filtering

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
LI MENG等: "Aerodynamic Parameter Estimation of an Unmanned Aerial Vehicle Based on Extended Kalman Filter and Its Higher Order Approach", 《IEEE》, 17 June 2010 (2010-06-17), pages 526 - 531 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116192571A (en) * 2023-02-06 2023-05-30 中国人民解放军火箭军工程大学 Unmanned aerial vehicle ISAC channel estimation method under beam dithering effect
CN116192571B (en) * 2023-02-06 2024-03-08 中国人民解放军火箭军工程大学 Unmanned aerial vehicle ISAC channel estimation method under beam dithering effect
CN117268409A (en) * 2023-08-30 2023-12-22 苏州大成运和智能科技有限公司 Unmanned vehicle local positioning method based on iterative extended Kalman filtering

Similar Documents

Publication Publication Date Title
CN109211276B (en) SINS initial alignment method based on GPR and improved SRCKF
CN106643737B (en) Four-rotor aircraft attitude calculation method in wind power interference environment
CN106052685B (en) A kind of posture and course estimation method of two-stage separation fusion
CN115033844A (en) Unmanned aerial vehicle state estimation method, system and device and readable storage medium
CN111102978A (en) Method and device for determining vehicle motion state and electronic equipment
Stovner et al. Attitude estimation by multiplicative exogenous Kalman filter
WO2022160391A1 (en) Magnetometer information assisted mems gyroscope calibration method and calibration system
CN111189442B (en) CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
CN108458709B (en) Airborne distributed POS data fusion method and device based on vision-aided measurement
Troni et al. Adaptive Estimation of Measurement Bias in Three-Dimensional Field Sensors with Angular Rate Sensors: Theory and Comparative Experimental Evaluation.
CN116147624B (en) Ship motion attitude calculation method based on low-cost MEMS navigation attitude reference system
CN110954102A (en) Magnetometer-assisted inertial navigation system and method for robot positioning
CN111190207B (en) PSTCSDREF algorithm-based unmanned aerial vehicle INS BDS integrated navigation method
Jameian et al. A robust and fast self-alignment method for strapdown inertial navigation system in rough sea conditions
CN113483765B (en) Satellite autonomous attitude determination method
CN112729332B (en) Alignment method based on rotation modulation
CN111982126A (en) Design method of full-source BeiDou/SINS elastic state observer model
CN117213480A (en) Transfer alignment method, system, equipment and storage medium
Blachuta et al. Attitude and heading reference system based on 3D complementary filter
CN112683265B (en) MIMU/GPS integrated navigation method based on rapid ISS collective filtering
Hua et al. Introduction to nonlinear attitude estimation for aerial robotic systems
Ma et al. Joint unscented Kalman filter for dual estimation in a bifilar pendulum for a small UAV
Zhang et al. An improved Kalman filter for attitude determination of multi-rotor UAVs based on low-cost MEMS sensors
CN113984042B (en) Series combined navigation method applicable to high-dynamic aircraft

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