CN113390421A - Unmanned aerial vehicle positioning method and device based on Kalman filtering - Google Patents

Unmanned aerial vehicle positioning method and device based on Kalman filtering Download PDF

Info

Publication number
CN113390421A
CN113390421A CN202110943418.6A CN202110943418A CN113390421A CN 113390421 A CN113390421 A CN 113390421A CN 202110943418 A CN202110943418 A CN 202110943418A CN 113390421 A CN113390421 A CN 113390421A
Authority
CN
China
Prior art keywords
covariance matrix
position value
prior
aerial vehicle
unmanned aerial
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110943418.6A
Other languages
Chinese (zh)
Other versions
CN113390421B (en
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.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
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 Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN202110943418.6A priority Critical patent/CN113390421B/en
Publication of CN113390421A publication Critical patent/CN113390421A/en
Application granted granted Critical
Publication of CN113390421B publication Critical patent/CN113390421B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • 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/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

The invention provides a Kalman filtering-based unmanned aerial vehicle positioning method and device, wherein the method comprises the following steps: sampling the current position value of the unmanned aerial vehicle based on a preset sampling method to obtain a plurality of sampling points; determining a prior position value and a prior covariance matrix of the unmanned aerial vehicle at the next moment based on the plurality of sampling points; determining the length of a next sliding window at the next moment according to the length of the current sliding window at the current moment; determining an estimated system noise covariance matrix and an estimated measurement noise covariance matrix of the next time according to the length of the next sliding window; and obtaining an observation position value at the next moment, and determining a posterior position value and a posterior covariance matrix at the next moment according to the observation position value, the estimation system noise covariance matrix, the estimation measurement noise covariance matrix, the prior position value and the prior covariance matrix so as to position the unmanned aerial vehicle. The invention improves the positioning accuracy of the unmanned aerial vehicle.

Description

Unmanned aerial vehicle positioning method and device based on Kalman filtering
Technical Field
The invention relates to the technical field of unmanned aerial vehicle positioning, in particular to an unmanned aerial vehicle positioning method and device based on Kalman filtering.
Background
With the rapid development of the unmanned aerial vehicle technology, the unmanned aerial vehicle is completely exposed in all fields, and the demand of all industries on the unmanned aerial vehicle is greatly increased. The unmanned aerial vehicle is not only applied to pesticide spraying and fertilizing, but also is an important navigator for maritime search and rescue. The positioning accuracy of the unmanned aerial vehicle determines the maritime search and rescue efficiency, and the unmanned aerial vehicle with lower positioning accuracy wastes a large amount of manpower and material resources and valuable search and rescue time.
In the prior art, an algorithm for positioning an unmanned aerial vehicle is a traditional kalman filtering algorithm, and the traditional kalman filtering assumes that a variance of system noise is a constant; in practical applications, the statistical characteristics of the system noise are difficult to obtain accurately and are dynamically changed. Therefore, assuming that the system noise covariance matrix is a constant, the set system noise covariance matrix value is not matched with the true value, so that the positioning accuracy of the unmanned aerial vehicle is seriously affected, and the technical problem of inaccurate positioning of the unmanned aerial vehicle is caused.
Disclosure of Invention
In view of the above, it is necessary to provide a method and a device for positioning an unmanned aerial vehicle based on kalman filtering, so as to solve the technical problem in the prior art that the positioning of the unmanned aerial vehicle is inaccurate.
In order to solve the technical problem, the invention provides an unmanned aerial vehicle positioning method based on Kalman filtering, which comprises the following steps:
sampling the current position value of the unmanned aerial vehicle based on a preset sampling method to obtain a plurality of sampling points;
determining a prior position value and a prior covariance matrix of the unmanned aerial vehicle at the next moment based on the plurality of sampling points;
determining the length of a next sliding window at the next moment according to the length of the current sliding window at the current moment;
determining an estimated system noise covariance matrix and an estimated measurement noise covariance matrix of the next time according to the length of the next sliding window;
and obtaining an observation position value at the next moment, and determining a posterior position value and a posterior covariance matrix at the next moment according to the observation position value, the estimation system noise covariance matrix, the estimation measurement noise covariance matrix, the prior position value and the prior covariance matrix so as to position the unmanned aerial vehicle.
In a possible implementation manner, the determining, based on the plurality of sampling points, an a priori position value and an a priori covariance matrix at a next time of the drone includes:
determining a first order statistical weight coefficient and a second order statistical weight coefficient of the plurality of sampling points;
determining a plurality of one-step predicted sampling points of the plurality of sampling points;
and determining the prior position value and the prior covariance matrix according to the first-order statistical weight coefficient, the second-order statistical weight coefficient and the plurality of one-step prediction sampling points.
In a possible implementation manner, the determining a next sliding window length of the next time according to the current sliding window length of the current time includes:
acquiring a current observation value at the current moment;
determining a current residual sequence of the current moment according to the current observation value, the current position value and the measurement equation;
determining historical average residual sequences of all historical moments;
and determining the length of the next sliding window according to the length of the current sliding window, the sequence of the current residual error and the sequence of the historical average residual error.
In one possible implementation, the determining the posterior position value and the posterior covariance matrix at the next time according to the observed position value, the estimated system noise covariance matrix, the estimated measured noise covariance matrix, the prior position value, and the prior covariance matrix includes:
determining an observation covariance matrix and a measurement covariance matrix according to the estimated system noise covariance matrix and the estimated measurement noise covariance matrix;
determining a Kalman gain matrix according to the observation covariance matrix and the measurement covariance matrix;
determining the posterior position value according to the prior position value, the observation position value and the Kalman gain matrix;
and determining the posterior covariance matrix according to the prior covariance matrix, the Kalman gain matrix and the observation covariance matrix.
In one possible implementation, the sampling points are:
Figure 228693DEST_PATH_IMAGE001
Figure 189696DEST_PATH_IMAGE002
Figure 962480DEST_PATH_IMAGE003
in the formula (I), the compound is shown in the specification,
Figure 768762DEST_PATH_IMAGE004
the sampling points are the sampling points;
Figure 677812DEST_PATH_IMAGE005
is the prior location value;
Figure 544137DEST_PATH_IMAGE006
is the dimension of the equation of state;
Figure 804217DEST_PATH_IMAGE007
the distance between the sampling point and the current position value is a parameter; pkIs the covariance matrix at time k.
In one possible implementation, the prior location value and the prior covariance matrix are respectively:
Figure 679769DEST_PATH_IMAGE008
Figure 443325DEST_PATH_IMAGE009
Figure 480552DEST_PATH_IMAGE010
Figure 696769DEST_PATH_IMAGE011
Figure 110433DEST_PATH_IMAGE012
in the formula (I), the compound is shown in the specification,
Figure 728496DEST_PATH_IMAGE013
is the prior location value;
Figure 202203DEST_PATH_IMAGE014
is the prior covariance matrix;
Figure 436875DEST_PATH_IMAGE015
is the first order statistical weight coefficient;
Figure 185388DEST_PATH_IMAGE016
is the second order statistical weight coefficient;
Figure 209624DEST_PATH_IMAGE017
predicting sampling points for the step;
Figure 588653DEST_PATH_IMAGE018
a one-step prediction function for said sample points;
Figure 45042DEST_PATH_IMAGE019
is a first white noise sequence;
Figure 534929DEST_PATH_IMAGE020
is the current measured noise covariance matrix at time k.
In one possible implementation, the next sliding window length is:
Figure 862005DEST_PATH_IMAGE021
Figure 677515DEST_PATH_IMAGE022
in the formula (I), the compound is shown in the specification,
Figure 355621DEST_PATH_IMAGE023
is the next sliding window length;
Figure 180357DEST_PATH_IMAGE024
is the current sliding window length; l is the measurement dimension;
Figure 486574DEST_PATH_IMAGE025
the current residual error sequence is obtained;
Figure 207405DEST_PATH_IMAGE026
is the jth element in the current residual error sequence;
Figure 372807DEST_PATH_IMAGE027
the j element in the historical average residual error sequence is obtained;
Figure 204497DEST_PATH_IMAGE028
is the current observation;
Figure 37324DEST_PATH_IMAGE029
is the current position value;
Figure 663477DEST_PATH_IMAGE030
mapping relation between the measured position value and the observed position value; and | represents rounding.
In one possible implementation, the posterior position values and the posterior covariance matrix are respectively:
Figure 581755DEST_PATH_IMAGE031
Figure 482714DEST_PATH_IMAGE032
Figure 907398DEST_PATH_IMAGE033
Figure 235611DEST_PATH_IMAGE034
Figure 375606DEST_PATH_IMAGE035
Figure 80256DEST_PATH_IMAGE036
Figure 622096DEST_PATH_IMAGE037
Figure 121211DEST_PATH_IMAGE038
Figure 482922DEST_PATH_IMAGE039
Figure 725684DEST_PATH_IMAGE040
in the formula (I), the compound is shown in the specification,
Figure 122031DEST_PATH_IMAGE041
is the posterior position value;
Figure 526467DEST_PATH_IMAGE042
is the prior location value;
Figure 641054DEST_PATH_IMAGE043
is the Kalman gain matrix;
Figure 421928DEST_PATH_IMAGE044
is the observed position value;
Figure 938360DEST_PATH_IMAGE045
is an estimated location value;
Figure 779277DEST_PATH_IMAGE046
is the posterior covariance matrix;
Figure 381160DEST_PATH_IMAGE047
an estimated covariance matrix for the prior covariance matrix;
Figure 965725DEST_PATH_IMAGE048
a transposed matrix that is the Kalman gain matrix;
Figure 336663DEST_PATH_IMAGE049
for the purpose of the observed covariance matrix,
Figure 79973DEST_PATH_IMAGE050
is the measured covariance matrix;
Figure 903572DEST_PATH_IMAGE051
is a second white noise sequence;
Figure 26249DEST_PATH_IMAGE052
estimating a system noise covariance matrix;
Figure 251694DEST_PATH_IMAGE053
to estimate a measured noise covariance matrix.
In a possible implementation manner, the estimated system noise covariance matrix and the estimated metrology noise covariance matrix are respectively:
Figure 434414DEST_PATH_IMAGE054
Figure 10888DEST_PATH_IMAGE055
Figure 937256DEST_PATH_IMAGE056
Figure 548366DEST_PATH_IMAGE057
Figure 901987DEST_PATH_IMAGE058
in the formula (I), the compound is shown in the specification,
Figure 700179DEST_PATH_IMAGE059
is an innovation sequence at the kth moment;
Figure 164658DEST_PATH_IMAGE060
is an innovation sequence at the k-i time;
Figure 833537DEST_PATH_IMAGE061
the residual error sequence at the k-i moment is obtained;
Figure 623638DEST_PATH_IMAGE062
is the prior position value at the k-th time.
On the other hand, the invention also provides an unmanned aerial vehicle positioning device based on Kalman filtering, which comprises:
the sampling unit is used for sampling the current position value of the unmanned aerial vehicle based on a preset sampling method to obtain a plurality of sampling points;
the prior value determining unit is used for determining a prior position value and a prior covariance matrix of the unmanned aerial vehicle at the next moment based on the plurality of sampling points;
a window length updating unit, configured to determine a next sliding window length at the next time according to the current sliding window length at the current time;
a noise covariance matrix determining unit, configured to determine an estimated system noise covariance matrix and an estimated measured noise covariance matrix at the next time according to the length of the next sliding window;
and the positioning unit is used for acquiring an observation position value at the next moment, and determining a posterior position value and a posterior covariance matrix at the next moment according to the observation position value, the estimated system noise covariance matrix, the estimated measurement noise covariance matrix, the prior position value and the prior covariance matrix so as to position the unmanned aerial vehicle.
The beneficial effects of adopting the above embodiment are: according to the unmanned aerial vehicle positioning method based on Kalman filtering, the current position value is sampled through a preset sampling method, a state equation and a measurement equation do not need to be subjected to linear approximation, errors generated in the linearization process of a branching system are avoided, and the unmanned aerial vehicle positioning accuracy is improved. Furthermore, the invention determines the length of the sliding window at the next moment, determines the estimated system noise covariance matrix and the estimated measurement noise covariance matrix at the next moment through the length of the sliding window, and determines the posterior position value and the posterior covariance matrix at the next moment according to the estimated system noise covariance matrix and the estimated measurement noise covariance matrix, so that the noise can be more matched with the real noise, and the positioning accuracy of the unmanned aerial vehicle is further improved.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.
Fig. 1 is a schematic flow chart of an embodiment of a kalman filtering-based unmanned aerial vehicle positioning method provided by the present invention;
FIG. 2 is a schematic flow chart of one embodiment of S102 of FIG. 1;
FIG. 3 is a schematic flow chart of one embodiment of S103 of FIG. 1;
FIG. 4 is a schematic flow chart of one embodiment of S105 of FIG. 1;
fig. 5 is a schematic structural diagram of an embodiment of the positioning device of the unmanned aerial vehicle based on kalman filtering provided by the present invention;
fig. 6 is a schematic structural diagram of an embodiment of an electronic device provided in the present invention.
Detailed Description
The technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. It is to be understood that the described embodiments are merely exemplary of the invention, and not restrictive of the full scope of the 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.
Reference herein to "an embodiment" means that a particular feature, structure, or characteristic described in connection with the embodiment can be included in at least one embodiment of the invention. The appearances of the phrase in various places in the specification are not necessarily all referring to the same embodiment, nor are separate or alternative embodiments mutually exclusive of other embodiments. It is explicitly and implicitly understood by one skilled in the art that the embodiments described herein can be combined with other embodiments.
The invention provides a Kalman filtering-based unmanned aerial vehicle positioning method and a Kalman filtering-based unmanned aerial vehicle positioning device, which are respectively explained below.
Fig. 1 is a schematic flow chart of an embodiment of the method for positioning an unmanned aerial vehicle based on kalman filtering, shown in fig. 1, the method includes:
s101, sampling the current position value of the unmanned aerial vehicle based on a preset sampling method to obtain a plurality of sampling points;
s102, determining a prior position value and a prior covariance matrix of the unmanned aerial vehicle at the next moment based on a plurality of sampling points;
s103, determining the length of a next sliding window at the next moment according to the length of the current sliding window at the current moment;
s104, determining an estimated system noise covariance matrix and an estimated measurement noise covariance matrix at the next moment according to the length of a next sliding window;
and S105, acquiring an observation position value at the next moment, and determining a posterior position value and a posterior covariance matrix at the next moment according to the observation position value, the estimated system noise covariance matrix, the estimated measurement noise covariance matrix, the prior position value and the prior covariance matrix so as to position the unmanned aerial vehicle.
Compared with the prior art, the unmanned aerial vehicle positioning method based on Kalman filtering provided by the embodiment of the invention samples the current position value by a preset sampling method, and does not need to carry out linear approximation on a state equation and a measurement equation, so that errors generated in the linearization process of a branching linear system are avoided, and the positioning accuracy of the unmanned aerial vehicle is improved. Furthermore, the invention determines the length of the sliding window at the next moment, determines the estimated system noise covariance matrix and the estimated measurement noise covariance matrix at the next moment through the length of the sliding window, and determines the posterior position value and the posterior covariance matrix at the next moment according to the estimated system noise covariance matrix and the estimated measurement noise covariance matrix, so that the noise can be more matched with the real noise, and the positioning accuracy of the unmanned aerial vehicle is further improved.
In some embodiments of the present invention, the current position value of the drone is obtained through a state equation and a measurement equation of the drone, specifically, the state equation of the drone is:
Figure 643547DEST_PATH_IMAGE063
in the formula (I), the compound is shown in the specification,
Figure 442876DEST_PATH_IMAGE064
measuring a position value at the k +1 th moment of the unmanned aerial vehicle;
Figure 231840DEST_PATH_IMAGE065
the measured value at the kth moment of the unmanned aerial vehicle is obtained;
Figure 726931DEST_PATH_IMAGE066
is a transition matrix;
Figure 234136DEST_PATH_IMAGE067
is a noise matrix;
Figure 305997DEST_PATH_IMAGE068
is the time difference between the kth time and the (k + 1) th time;
Figure 480626DEST_PATH_IMAGE069
is an identity matrix.
In some embodiments of the invention, the measurement equation is:
Figure 81372DEST_PATH_IMAGE070
Figure 75873DEST_PATH_IMAGE071
Figure 685846DEST_PATH_IMAGE072
in the formula (I), the compound is shown in the specification,
Figure 714981DEST_PATH_IMAGE073
the observed position value at the kth moment is;
Figure 752208DEST_PATH_IMAGE074
the measured position value at the kth moment;
Figure 499584DEST_PATH_IMAGE075
mapping relation between the observation position value and the measurement position value;
Figure 178827DEST_PATH_IMAGE076
is the azimuth of the drone;
Figure 62469DEST_PATH_IMAGE077
is the altitude angle of the unmanned aerial vehicle; t is tx，ty，tzObserving the position of the unmanned aerial vehicle for the camera at the kth moment; p is a radical ofx，py，pzIs the self position of the unmanned plane.
Wherein, measuration position value accessible is installed the gyroscope on the unmanned aerial vehicle and is obtained.
It should be noted that: t is tx，ty，tz，px，py，pzAll are coordinate values under the self three-dimensional coordinate system of the unmanned aerial vehicle, then tx，ty，tzCan be obtained from the following coordinate transformation:
Figure 536176DEST_PATH_IMAGE078
in the formula, tpThe coordinate of the central point of the camera view field in the three-dimensional coordinate system of the camera is shown,
Figure 239690DEST_PATH_IMAGE079
is a rotation matrix from a three-dimensional coordinate system of the unmanned plane to a geographic coordinate system,
Figure 925886DEST_PATH_IMAGE080
the rotation matrix from the three-dimensional coordinate system where the camera is located to the three-dimensional coordinate system of the unmanned aerial vehicle.
In particular, tpIs (0,0, f), where f is the focal length of the camera.
The measurement equation can be constructed by observing the position coordinate of the unmanned aerial vehicle and the position coordinate of the unmanned aerial vehicle through the camera in the same three-dimensional coordinate system.
In some embodiments of the present invention, as shown in fig. 2, step S102 comprises:
s201, determining a first-order statistical weight coefficient and a second-order statistical weight coefficient of a plurality of sampling points;
s202, determining a plurality of one-step prediction sampling points of a plurality of sampling points;
s203, determining a prior position value and a prior covariance matrix according to the first-order statistical weight coefficient, the second-order statistical weight coefficient and the plurality of one-step prediction sampling points.
It should be noted that, in the embodiment of the present invention, the preset sampling method is Sigma sampling.
Specifically, the sample points after sampling by Sigma are:
Figure 929614DEST_PATH_IMAGE081
Figure 43064DEST_PATH_IMAGE082
Figure 233873DEST_PATH_IMAGE083
in the formula (I), the compound is shown in the specification,
Figure 989340DEST_PATH_IMAGE084
sampling points are obtained;
Figure 110224DEST_PATH_IMAGE085
is a priori position value;
Figure 925733DEST_PATH_IMAGE086
is the dimension of the equation of state;
Figure 134998DEST_PATH_IMAGE087
is a parameter of the distance between the sampling point and the current position value; pkIs the covariance matrix at time k.
Wherein, the first order statistical weight coefficient and the second order statistical weight coefficient are respectively:
Figure 428576DEST_PATH_IMAGE088
in the formula (I), the compound is shown in the specification,
Figure 141317DEST_PATH_IMAGE089
is a first order statistical weight coefficient;
Figure 127727DEST_PATH_IMAGE090
is a second order statistical weight coefficient.
Wherein, the one-step prediction sampling point is as follows:
Figure 293130DEST_PATH_IMAGE091
in the formula (I), the compound is shown in the specification,
Figure 124819DEST_PATH_IMAGE092
predicting sampling points for one step;
Figure 692067DEST_PATH_IMAGE093
is a one-step predictive function of the sample points.
After the sampling point, the first order statistical weight coefficient, the second order statistical weight coefficient and the one-step prediction sampling point are obtained through the formula calculation, the prior position value and the prior covariance matrix are respectively as follows:
Figure 849379DEST_PATH_IMAGE094
Figure 33235DEST_PATH_IMAGE095
Figure 403037DEST_PATH_IMAGE096
in the formula (I), the compound is shown in the specification,
Figure 90370DEST_PATH_IMAGE097
is a priori position value;
Figure 153004DEST_PATH_IMAGE098
is a prior covariance matrix;
Figure 27419DEST_PATH_IMAGE099
is a first white noise sequence;
Figure 732070DEST_PATH_IMAGE100
is the current measured noise covariance matrix at time k.
In some embodiments of the present invention, as shown in fig. 3, step S103 comprises:
s301, acquiring a current observation value at the current moment;
s302, determining a current residual sequence at the current moment according to the current observation value and the current position value;
s303, determining historical average residual sequences of all historical moments;
and S304, determining the length of the next sliding window according to the length of the current sliding window, the current residual sequence and the historical average residual sequence.
Specifically, the next sliding window length is:
Figure 8331DEST_PATH_IMAGE101
Figure 241866DEST_PATH_IMAGE102
in the formula (I), the compound is shown in the specification,
Figure 872086DEST_PATH_IMAGE103
is the next sliding window length;
Figure 583690DEST_PATH_IMAGE104
is the current sliding window length; l is the measurement dimension;
Figure 980036DEST_PATH_IMAGE105
is the current residual error sequence;
Figure 650052DEST_PATH_IMAGE106
is the jth element in the current residual error sequence;
Figure 499059DEST_PATH_IMAGE107
is the jth element in the historical average residual error sequence;
Figure 14354DEST_PATH_IMAGE108
is the current observed value;
Figure 265207DEST_PATH_IMAGE109
is the current position value;
Figure 106124DEST_PATH_IMAGE110
mapping relation between the measured position value and the observed position value; and | represents rounding.
It should be noted that: to further improve the positioning accuracy, in some embodiments of the present invention, | | | represents rounding up.
Further, the estimated system noise covariance matrix and the estimated metrology noise covariance matrix are respectively:
Figure 442427DEST_PATH_IMAGE111
Figure 761413DEST_PATH_IMAGE112
Figure 866773DEST_PATH_IMAGE113
Figure 878591DEST_PATH_IMAGE114
Figure 436611DEST_PATH_IMAGE115
in the formula (I), the compound is shown in the specification,
Figure 559288DEST_PATH_IMAGE116
estimating a system noise covariance matrix;
Figure 784733DEST_PATH_IMAGE117
estimating a measured noise covariance matrix;
Figure 701873DEST_PATH_IMAGE118
is an innovation sequence at the kth moment;
Figure 12769DEST_PATH_IMAGE119
the prior position value at the kth moment;
Figure 939137DEST_PATH_IMAGE120
is an innovation sequence at the k-i time;
Figure 753509DEST_PATH_IMAGE121
is the residual sequence at time k-i.
It should be understood that: by dynamically adjusting the length of the next window at the next moment, the value of the residual sequence is small and changes stably, the width of the next sliding window is increased at the moment, and the frequency of the self-adaptive adjustment of the measurement noise and the system noise is reduced; when the change of the system noise is large, the width of the next sliding window is shortened, and the Sigma sampling frequency is increased, so that the estimated system noise matrix and the estimated measurement noise matrix can be closer to the characteristic of real noise.
In some embodiments of the present invention, as shown in fig. 4, step S105 comprises:
s401, determining an observation covariance matrix and a measurement covariance matrix according to the estimated system noise covariance matrix and the estimated measurement noise covariance matrix;
s402, determining a Kalman gain matrix according to the observation covariance matrix and the measurement covariance matrix;
s403, determining a posterior position value according to the prior position value, the observation position value and the Kalman gain matrix;
s404, determining a posterior covariance matrix according to the prior covariance matrix, the Kalman gain matrix and the observation covariance matrix.
Specifically, the posterior position value and the posterior covariance matrix are respectively:
Figure 107130DEST_PATH_IMAGE122
Figure 639742DEST_PATH_IMAGE123
Figure 104222DEST_PATH_IMAGE124
Figure 38680DEST_PATH_IMAGE125
Figure 563202DEST_PATH_IMAGE126
Figure 845760DEST_PATH_IMAGE127
Figure 113930DEST_PATH_IMAGE128
Figure 902895DEST_PATH_IMAGE129
Figure 332739DEST_PATH_IMAGE130
Figure 105523DEST_PATH_IMAGE131
in the formula (I), the compound is shown in the specification,
Figure 911805DEST_PATH_IMAGE132
is a posterior position value;
Figure 555276DEST_PATH_IMAGE133
is a priori position value;
Figure 421601DEST_PATH_IMAGE134
is a Kalman gain matrix;
Figure 416102DEST_PATH_IMAGE135
is an observed position value;
Figure 26075DEST_PATH_IMAGE136
is an estimated location value;
Figure 55211DEST_PATH_IMAGE137
is a posterior covariance matrix;
Figure 826857DEST_PATH_IMAGE138
an estimated covariance matrix that is a prior covariance matrix;
Figure 308654DEST_PATH_IMAGE139
a transposed matrix that is a Kalman gain matrix;
Figure 456739DEST_PATH_IMAGE140
in order to observe the covariance matrix,
Figure 340381DEST_PATH_IMAGE141
for measuring covariance matrixArraying;
Figure 282930DEST_PATH_IMAGE142
is a second white noise sequence;
Figure 783181DEST_PATH_IMAGE116
estimating a system noise covariance matrix;
Figure 734957DEST_PATH_IMAGE117
to estimate a measured noise covariance matrix.
Wherein, obtain posterior position value and can fix a position unmanned aerial vehicle.
In order to better implement the kalman filter-based unmanned aerial vehicle positioning method in the embodiment of the present invention, on the basis of the kalman filter-based unmanned aerial vehicle positioning method, as shown in fig. 5, correspondingly, an embodiment of the present invention further provides an unmanned aerial vehicle positioning apparatus 500 based on the kalman filter, including:
the sampling unit 501 is configured to sample a current position value of the unmanned aerial vehicle based on a preset sampling method to obtain a plurality of sampling points;
a priori value determining unit 502, configured to determine a priori position value and a priori covariance matrix at a next moment of the unmanned aerial vehicle based on the multiple sampling points;
a window length updating unit 503, configured to determine a next sliding window length at a next time according to the current sliding window length at the current time;
a noise covariance matrix determination unit 504, configured to determine an estimated system noise covariance matrix and an estimated measured noise covariance matrix at a next time according to a next sliding window length;
and a positioning unit 505, configured to obtain an observed position value at a next time, and determine a posterior position value and a posterior covariance matrix at the next time according to the observed position value, the estimated system noise covariance matrix, the estimated measurement noise covariance matrix, the prior position value, and the prior covariance matrix, so as to position the unmanned aerial vehicle.
The kalman filtering-based unmanned aerial vehicle positioning apparatus 500 provided in the above-mentioned embodiment may implement the technical solution described in the above-mentioned kalman filtering-based unmanned aerial vehicle positioning method embodiment, and the principle of specifically implementing each module or unit may refer to the corresponding content in the above-mentioned kalman filtering-based unmanned aerial vehicle positioning method embodiment, and is not described here any more.
As shown in fig. 6, the present invention further provides an electronic device 600. The electronic device 600 comprises a processor 601, a memory 602 and a display 603. Fig. 6 shows only some of the components of the electronic device 600, but it is to be understood that not all of the shown components are required to be implemented, and that more or fewer components may be implemented instead.
The storage 602 may be an internal storage unit of the electronic device 600 in some embodiments, such as a hard disk or a memory of the electronic device 600. The memory 602 may also be an external storage device of the electronic device 600 in other embodiments, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), and the like, provided on the electronic device 600.
Further, the memory 602 may also include both internal storage units and external storage devices of the electronic device 600. The memory 602 is used for storing application software and various types of data for installing the electronic device 600.
Processor 601, which in some embodiments may be a Central Processing Unit (CPU), microprocessor or other data Processing chip, is configured to run program code stored in memory 602 or process data, such as the kalman filter-based drone positioning method of the present invention.
The display 603 may be an LED display, a liquid crystal display, a touch-sensitive liquid crystal display, an OLED (Organic Light-Emitting Diode) touch panel, or the like in some embodiments. The display 603 is used for displaying information at the electronic device 600 and for displaying a visual user interface. The components 601 and 603 of the electronic device 600 communicate with each other via a system bus.
In an embodiment, when processor 601 executes a kalman filter based drone positioning program in memory 602, the following steps may be implemented:
sampling the current position value of the unmanned aerial vehicle based on a preset sampling method to obtain a plurality of sampling points;
determining a prior position value and a prior covariance matrix of the unmanned aerial vehicle at the next moment based on the plurality of sampling points;
determining the length of a next sliding window at the next moment according to the length of the current sliding window at the current moment;
determining an estimated system noise covariance matrix and an estimated measurement noise covariance matrix at the next moment according to the length of the next sliding window;
and acquiring an observation position value at the next moment, and determining a posterior position value and a posterior covariance matrix at the next moment according to the observation position value, the estimated system noise covariance matrix, the estimated measurement noise covariance matrix, the prior position value and the prior covariance matrix so as to position the unmanned aerial vehicle.
It should be understood that: when the processor 601 executes the kalman filter-based drone positioning program in the memory 602, in addition to the above functions, other functions may be implemented, as specifically described above with reference to the corresponding method embodiments.
Further, the type of the electronic device 600 is not particularly limited in the embodiment of the present invention, and the electronic device 600 may be a portable electronic device such as a mobile phone, a tablet computer, a Personal Digital Assistant (PDA), a wearable device, and a laptop computer (laptop). Exemplary embodiments of portable electronic devices include, but are not limited to, portable electronic devices that carry an IOS, android, microsoft, or other operating system. The portable electronic device may also be other portable electronic devices such as laptop computers (laptop) with touch sensitive surfaces (e.g., touch panels), etc. It should also be understood that in other embodiments of the present invention, the electronic device 600 may not be a portable electronic device, but may be a desktop computer having a touch-sensitive surface (e.g., a touch panel).
Accordingly, the present application also provides a computer-readable storage medium, which is used for storing a computer-readable program or instruction, and when the program or instruction is executed by a processor, the method steps or functions provided by the above method embodiments can be implemented.
Those skilled in the art will appreciate that all or part of the flow of the method implementing the above embodiments may be implemented by a computer program instructing associated hardware, and the program may be stored in a computer-readable storage medium. The computer readable storage medium is a magnetic disk, an optical disk, a read-only memory or a random access memory.
The method and the device for positioning the unmanned aerial vehicle based on the kalman filter are described in detail, and a specific example is applied in the method to explain the principle and the implementation mode of the method, and the description of the embodiment is only used for helping to understand the method and the core idea of the method; meanwhile, for those skilled in the art, according to the idea of the present invention, there may be variations in the specific embodiments and the application scope, and in summary, the content of the present specification should not be construed as a limitation to the present invention.

Claims (10)

1. An unmanned aerial vehicle positioning method based on Kalman filtering is characterized by comprising the following steps:
sampling the current position value of the unmanned aerial vehicle based on a preset sampling method to obtain a plurality of sampling points;
determining a prior position value and a prior covariance matrix of the unmanned aerial vehicle at the next moment based on the plurality of sampling points;
determining the length of a next sliding window at the next moment according to the length of the current sliding window at the current moment;
determining an estimated system noise covariance matrix and an estimated measurement noise covariance matrix of the next time according to the length of the next sliding window;
and obtaining an observation position value at the next moment, and determining a posterior position value and a posterior covariance matrix at the next moment according to the observation position value, the estimation system noise covariance matrix, the estimation measurement noise covariance matrix, the prior position value and the prior covariance matrix so as to position the unmanned aerial vehicle.
2. The Kalman filtering based unmanned aerial vehicle positioning method of claim 1, wherein the determining of the prior position value and the prior covariance matrix of the unmanned aerial vehicle at the next moment based on the plurality of sampling points comprises:
determining a first order statistical weight coefficient and a second order statistical weight coefficient of the plurality of sampling points;
determining a plurality of one-step predicted sampling points of the plurality of sampling points;
and determining the prior position value and the prior covariance matrix according to the first-order statistical weight coefficient, the second-order statistical weight coefficient and the plurality of one-step prediction sampling points.
3. The Kalman filtering based unmanned aerial vehicle positioning method of claim 2, wherein the determining a next sliding window length for the next time instant according to the current sliding window length for the current time instant comprises:
acquiring a current observation value at the current moment;
determining a current residual sequence of the current moment according to the current observation value and the current position value;
determining historical average residual sequences of all historical moments;
and determining the length of the next sliding window according to the length of the current sliding window, the sequence of the current residual error and the sequence of the historical average residual error.
4. The Kalman filtering based UAV positioning method of claim 3, wherein the determining the posteriori position values and the posteriori covariance matrices for the next time instance based on the observed position values, the estimated system noise covariance matrix, the estimated metrology noise covariance matrix, the prior position values, and the prior covariance matrix comprises:
determining an observation covariance matrix and a measurement covariance matrix according to the estimated system noise covariance matrix and the estimated measurement noise covariance matrix;
determining a Kalman gain matrix according to the observation covariance matrix and the measurement covariance matrix;
determining the posterior position value according to the prior position value, the observation position value and the Kalman gain matrix;
and determining the posterior covariance matrix according to the prior covariance matrix, the Kalman gain matrix and the observation covariance matrix.
5. The Kalman filtering based unmanned aerial vehicle positioning method of claim 4, wherein the sampling points are:
Figure 925702DEST_PATH_IMAGE001
Figure 731984DEST_PATH_IMAGE002
Figure 643964DEST_PATH_IMAGE003
in the formula (I), the compound is shown in the specification,
Figure 510289DEST_PATH_IMAGE004
the sampling points are the sampling points;
Figure 239210DEST_PATH_IMAGE005
is the prior location value;
Figure 114763DEST_PATH_IMAGE006
is the dimension of the equation of state;
Figure 612740DEST_PATH_IMAGE007
is a stand forThe distance between the sampling point and the current position value is a parameter; pkIs the covariance matrix at time k.
6. The Kalman filtering based unmanned aerial vehicle positioning method of claim 5, wherein the prior position value and the prior covariance matrix are respectively:
Figure 649966DEST_PATH_IMAGE008
Figure 131763DEST_PATH_IMAGE009
Figure 279848DEST_PATH_IMAGE010
Figure 897911DEST_PATH_IMAGE011
Figure 371617DEST_PATH_IMAGE012
in the formula (I), the compound is shown in the specification,
Figure 75131DEST_PATH_IMAGE013
is the prior location value;
Figure 292486DEST_PATH_IMAGE014
is the prior covariance matrix;
Figure 765056DEST_PATH_IMAGE015
is the first order statistical weight coefficient;
Figure 675243DEST_PATH_IMAGE016
is the second order statistical weight coefficient;
Figure 866053DEST_PATH_IMAGE017
predicting sampling points for the step;
Figure 621519DEST_PATH_IMAGE018
a one-step prediction function for said sample points;
Figure 214174DEST_PATH_IMAGE019
is a first white noise sequence;
Figure 498525DEST_PATH_IMAGE020
is the current measured noise covariance matrix at time k.
7. The Kalman filtering based unmanned aerial vehicle positioning method of claim 6, wherein the next sliding window length is:
Figure 176631DEST_PATH_IMAGE021
Figure 470209DEST_PATH_IMAGE022
in the formula (I), the compound is shown in the specification,
Figure 182950DEST_PATH_IMAGE023
is the next sliding window length;
Figure 903782DEST_PATH_IMAGE024
is the current sliding window length; l is the measurement dimension;
Figure 803605DEST_PATH_IMAGE025
is as described inA pre-residual sequence;
Figure 897944DEST_PATH_IMAGE026
is the jth element in the current residual error sequence;
Figure 465192DEST_PATH_IMAGE027
the j element in the historical average residual error sequence is obtained;
Figure 356924DEST_PATH_IMAGE028
is the current observation;
Figure 744043DEST_PATH_IMAGE029
is the current position value;
Figure 645003DEST_PATH_IMAGE030
mapping relation between the measured position value and the observed position value; and | represents rounding.
8. The Kalman filtering based unmanned aerial vehicle positioning method of claim 7, wherein the posterior position value and the posterior covariance matrix are respectively:
Figure 66757DEST_PATH_IMAGE031
Figure 129391DEST_PATH_IMAGE032
Figure 269385DEST_PATH_IMAGE033
Figure 442878DEST_PATH_IMAGE034
Figure 719138DEST_PATH_IMAGE035
Figure 952674DEST_PATH_IMAGE036
Figure 314385DEST_PATH_IMAGE037
Figure 557147DEST_PATH_IMAGE038
Figure 687914DEST_PATH_IMAGE039
Figure 92351DEST_PATH_IMAGE040
in the formula (I), the compound is shown in the specification,
Figure 941358DEST_PATH_IMAGE041
is the posterior position value;
Figure 987812DEST_PATH_IMAGE042
is the prior location value;
Figure 973085DEST_PATH_IMAGE043
is the Kalman gain matrix;
Figure 548423DEST_PATH_IMAGE044
is the observed position value;
Figure 884726DEST_PATH_IMAGE045
is an estimated location value;
Figure 203712DEST_PATH_IMAGE046
is the posterior covariance matrix;
Figure 574651DEST_PATH_IMAGE047
an estimated covariance matrix for the prior covariance matrix;
Figure 320890DEST_PATH_IMAGE048
a transposed matrix that is the Kalman gain matrix;
Figure 144489DEST_PATH_IMAGE049
for the purpose of the observed covariance matrix,
Figure 1587DEST_PATH_IMAGE050
is the measured covariance matrix;
Figure 961453DEST_PATH_IMAGE051
is a second white noise sequence;
Figure 412681DEST_PATH_IMAGE052
estimating a system noise covariance matrix;
Figure 457998DEST_PATH_IMAGE053
to estimate a measured noise covariance matrix.
9. The kalman filter-based unmanned aerial vehicle positioning method of claim 8, wherein the estimated system noise covariance matrix and the estimated metrology noise covariance matrix are respectively:
Figure 384365DEST_PATH_IMAGE054
Figure 464317DEST_PATH_IMAGE055
Figure 552359DEST_PATH_IMAGE056
Figure 350550DEST_PATH_IMAGE057
Figure 815030DEST_PATH_IMAGE058
in the formula (I), the compound is shown in the specification,
Figure 749488DEST_PATH_IMAGE059
is an innovation sequence at the kth moment;
Figure 274010DEST_PATH_IMAGE060
is an innovation sequence at the k-i time;
Figure 293918DEST_PATH_IMAGE061
the residual error sequence at the k-i moment is obtained;
Figure 296510DEST_PATH_IMAGE062
is the prior position value at the k-th time.
10. The utility model provides an unmanned aerial vehicle positioner based on kalman filter, its characterized in that includes:
the sampling unit is used for sampling the current position value of the unmanned aerial vehicle based on a preset sampling method to obtain a plurality of sampling points;
the prior value determining unit is used for determining a prior position value and a prior covariance matrix of the unmanned aerial vehicle at the next moment based on the plurality of sampling points;
a window length updating unit, configured to determine a next sliding window length at the next time according to the current sliding window length at the current time;
a noise covariance matrix determining unit, configured to determine an estimated system noise covariance matrix and an estimated measured noise covariance matrix at the next time according to the length of the next sliding window;
and the positioning unit is used for acquiring an observation position value at the next moment, and determining a posterior position value and a posterior covariance matrix at the next moment according to the observation position value, the estimated system noise covariance matrix, the estimated measurement noise covariance matrix, the prior position value and the prior covariance matrix so as to position the unmanned aerial vehicle.
CN202110943418.6A 2021-08-17 2021-08-17 Unmanned aerial vehicle positioning method and device based on Kalman filtering Active CN113390421B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110943418.6A CN113390421B (en) 2021-08-17 2021-08-17 Unmanned aerial vehicle positioning method and device based on Kalman filtering

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110943418.6A CN113390421B (en) 2021-08-17 2021-08-17 Unmanned aerial vehicle positioning method and device based on Kalman filtering

Publications (2)

Publication Number Publication Date
CN113390421A true CN113390421A (en) 2021-09-14
CN113390421B CN113390421B (en) 2021-12-10

Family

ID=77622715

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110943418.6A Active CN113390421B (en) 2021-08-17 2021-08-17 Unmanned aerial vehicle positioning method and device based on Kalman filtering

Country Status (1)

Country Link
CN (1) CN113390421B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115469553A (en) * 2022-11-02 2022-12-13 中国船舶集团有限公司第七〇七研究所 Ship motion state reconstruction method, device, equipment and storage medium

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101464152A (en) * 2009-01-09 2009-06-24 哈尔滨工程大学 Adaptive filtering method for SINS/GPS combined navigation system
CN107169478A (en) * 2017-06-26 2017-09-15 北京工商大学 A kind of adaptive online filtering method
CN107300697A (en) * 2017-06-07 2017-10-27 南京航空航天大学 Moving target UKF filtering methods based on unmanned plane
CN108128308A (en) * 2017-12-27 2018-06-08 长沙理工大学 A kind of vehicle state estimation system and method for distributed-driving electric automobile
CN112683274A (en) * 2020-12-22 2021-04-20 西安因诺航空科技有限公司 Unmanned aerial vehicle integrated navigation method and system based on unscented Kalman filtering

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101464152A (en) * 2009-01-09 2009-06-24 哈尔滨工程大学 Adaptive filtering method for SINS/GPS combined navigation system
CN107300697A (en) * 2017-06-07 2017-10-27 南京航空航天大学 Moving target UKF filtering methods based on unmanned plane
CN107169478A (en) * 2017-06-26 2017-09-15 北京工商大学 A kind of adaptive online filtering method
CN108128308A (en) * 2017-12-27 2018-06-08 长沙理工大学 A kind of vehicle state estimation system and method for distributed-driving electric automobile
CN112683274A (en) * 2020-12-22 2021-04-20 西安因诺航空科技有限公司 Unmanned aerial vehicle integrated navigation method and system based on unscented Kalman filtering

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
BIN LI .ET AL: "Adaptive Extended Kalman Filtering Algorithm for SINS/GPS Integrated Navigation in Guided Munitions", 《2010 IEEE INTERNATIONAL CONFERENCE ON INTELLIGENT COMPUTING AND INTELLIGENT SYSTEMS》 *
唐大全等: "基于迭代无迹卡尔曼滤波的小型无人机目标定位方法", 《指挥控制与仿真》 *
张志勇等: "基于自适应扩展卡尔曼滤波的分布式驱动电动汽车状态估计", 《机械工程学报》 *
石喜玲等: "基于UKF信息融合的多旋翼飞行器姿态估计", 《测试技术学报》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115469553A (en) * 2022-11-02 2022-12-13 中国船舶集团有限公司第七〇七研究所 Ship motion state reconstruction method, device, equipment and storage medium

Also Published As

Publication number Publication date
CN113390421B (en) 2021-12-10

Similar Documents

Publication Publication Date Title
CN110866496B (en) Robot positioning and mapping method and device based on depth image
US20140341465A1 (en) Real-time pose estimation system using inertial and feature measurements
CN107255795A (en) Localization Approach for Indoor Mobile and device based on EKF/EFIR mixed filterings
CN111460375A (en) Positioning data validity determination method, device, equipment and medium
CN113390421B (en) Unmanned aerial vehicle positioning method and device based on Kalman filtering
CN112082547A (en) Integrated navigation system optimization method and device, electronic equipment and storage medium
CN115711616B (en) Smooth positioning method and device for indoor and outdoor traversing unmanned aerial vehicle
CN114926549B (en) Three-dimensional point cloud processing method, device, equipment and storage medium
CN114413898B (en) Multi-sensor data fusion method and device, computer equipment and storage medium
CN114485594B (en) Antenna pose information measuring method, apparatus, device, medium, and program product
CN115457152A (en) External parameter calibration method and device, electronic equipment and storage medium
EP2972683A1 (en) Dynamically calibrating magnetic sensors
CN115164936A (en) Global pose correction method and device for point cloud splicing in high-precision map manufacturing
Zhao et al. Robust adaptive heading tracking fusion for polarization compass with uncertain dynamics and external disturbances
CN117948965A (en) Point cloud track optimization method and device, electronic equipment and storage medium
CN108981689B (en) UWB/INS combined navigation system based on DSP TMS320C6748
CN108253931B (en) Binocular stereo vision ranging method and ranging device thereof
CN118057120A (en) Method and apparatus for estimating device pose
CN112697146B (en) Steady regression-based track prediction method
EP2972682B1 (en) Computing a magnetic heading
CN103206953A (en) Method of generating geometric heading and positioning system using the same method
Qi et al. Continuous self‐calibration of platform inertial navigation system based on attitude quaternion model
CN112556722B (en) System error compensation method based on automatic selection of preferred sources
CN112364292B (en) Ransac-based dense target tracking method, ransac-based dense target tracking device, ransac-based dense target tracking equipment and medium
CN115420418B (en) Air pressure measuring method and device, electronic equipment and readable storage medium

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant