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
current
prior
sliding window
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

本发明提供了一种基于卡尔曼滤波的无人机定位方法及装置，其方法包括：基于预设采样方法对无人机的当前位置值进行采样，获得多个采样点；基于所述多个采样点，确定所述无人机下一时刻的先验位置值和先验协方差矩阵；根据所述当前时刻的当前滑动窗口长度确定所述下一时刻的下一滑动窗口长度；根据所述下一滑动窗口长度确定所述下一时刻的估计系统噪声协方差矩阵和估计量测噪声协方差矩阵；获取下一时刻的观测位置值，并根据所述观测位置值、所述估计系统噪声协方差矩阵、所述估计量测噪声协方差矩阵、所述先验位置值以及所述先验协方差矩阵确定所述下一时刻的后验位置值和后验协方差矩阵，以对无人机定位。本发明提高了无人机定位的准确性。

Figure 202110943418

The present invention provides a UAV positioning method and device based on Kalman filtering. The method includes: sampling the current position value of the UAV based on a preset sampling method to obtain multiple sampling points; Sampling points, determine the prior position value and prior covariance matrix of the UAV at the next moment; determine the length of the next sliding window at the next moment according to the current sliding window length at the current moment; according to the The length of the next sliding window determines the estimated system noise covariance matrix and the estimated measurement noise covariance matrix at the next moment; The variance matrix, the estimated measurement noise covariance matrix, the prior position value, and the prior covariance matrix determine the posterior position value and the posterior covariance matrix at the next moment, so as to determine the a posteriori position value and the posterior covariance matrix for the UAV position. The invention improves the positioning accuracy of the unmanned aerial vehicle.

Figure 202110943418

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.一种基于卡尔曼滤波的无人机定位方法，其特征在于，包括：1. a UAV positioning method based on Kalman filtering, is characterized in that, comprises: 基于预设采样方法对无人机的当前位置值进行采样，获得多个采样点；Sampling the current position value of the UAV based on the preset sampling method to obtain multiple sampling points; 基于所述多个采样点，确定所述无人机下一时刻的先验位置值和先验协方差矩阵；Based on the plurality of sampling points, determine the prior position value and prior covariance matrix of the UAV at the next moment; 根据所述当前时刻的当前滑动窗口长度确定所述下一时刻的下一滑动窗口长度；Determine the length of the next sliding window at the next moment according to the current sliding window length at the current moment; 根据所述下一滑动窗口长度确定所述下一时刻的估计系统噪声协方差矩阵和估计量测噪声协方差矩阵；Determine the estimated system noise covariance matrix and the estimated measurement noise covariance matrix at the next moment according to the length of the next sliding window; 获取下一时刻的观测位置值，并根据所述观测位置值、所述估计系统噪声协方差矩阵、所述估计量测噪声协方差矩阵、所述先验位置值以及所述先验协方差矩阵确定所述下一时刻的后验位置值和后验协方差矩阵，以对无人机定位。Obtain the observation position value at the next moment, and 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 The posterior position value and posterior covariance matrix of the next moment are determined to locate the UAV. 2.根据权利要求1所述的基于卡尔曼滤波的无人机定位方法，其特征在于，所述基于所述多个采样点，确定所述无人机下一时刻的先验位置值和先验协方差矩阵包括：2. The UAV positioning method based on Kalman filtering according to claim 1, wherein, based on the plurality of sampling points, the prior position value and the prior position value of the UAV at the next moment are determined based on the plurality of sampling points. The empirical covariance matrix includes: 确定所述多个采样点的一阶统计权系数和二阶统计权系数；determining the first-order statistical weight coefficient and the second-order statistical weight coefficient of the plurality of sampling points; 确定所述多个采样点的多个一步预测采样点；determining a plurality of one-step prediction sampling points of the plurality of sampling points; 根据所述一阶统计权系数、所述二阶统计权系数以及所述多个一步预测采样点确定所述先验位置值和所述先验协方差矩阵。The prior position value and the prior covariance matrix are determined according to the first-order statistical weight coefficient, the second-order statistical weight coefficient, and the plurality of one-step prediction sampling points. 3.根据权利要求2所述的基于卡尔曼滤波的无人机定位方法，其特征在于，所述根据所述当前时刻的当前滑动窗口长度确定所述下一时刻的下一滑动窗口长度包括：3. The unmanned aerial vehicle positioning method based on Kalman filtering according to claim 2, wherein the determining the next sliding window length of the next moment according to the current sliding window length of the current moment comprises: 获取当前时刻的当前观测值；Get the current observation at the current moment; 根据所述当前观测值以及所述当前位置值确定所述当前时刻的当前残差序列；Determine the current residual sequence at the current moment according to the current observation value and the current position value; 确定所有历史时刻的历史平均残差序列；Determine the historical average residual sequence of all historical moments; 根据所述当前滑动窗口长度、所述当前残差序列以及所述历史平均残差序列确定所述下一滑动窗口长度。The next sliding window length is determined according to the current sliding window length, the current residual sequence and the historical average residual sequence. 4.根据权利要求3所述的基于卡尔曼滤波的无人机定位方法，其特征在于，所述根据所述观测位置值、所述估计系统噪声协方差矩阵、所述估计量测噪声协方差矩阵、所述先验位置值以及所述先验协方差矩阵确定所述下一时刻的后验位置值和后验协方差矩阵包括：4. The unmanned aerial vehicle positioning method based on Kalman filtering according to claim 3, characterized in that, according to the observed position value, the estimated system noise covariance matrix, the estimated measurement noise covariance The matrix, the prior position value and the prior covariance matrix to determine the posterior position value and the posterior covariance matrix at the next moment include: 根据所述估计系统噪声协方差矩阵以及所述估计量测噪声协方差矩阵确定观测协方差矩阵和量测协方差矩阵；Determine an observation covariance matrix and a measurement covariance matrix according to the estimated system noise covariance matrix and the estimated measurement noise covariance matrix; 根据所述观测协方差矩阵以及所述量测协方差矩阵确定卡尔曼增益矩阵；Determine 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 observed position value and the Kalman gain matrix; 根据所述先验协方差矩阵、所述卡尔曼增益矩阵以及所述观测协方差矩阵确定所述后验协方差矩阵。The posterior covariance matrix is determined according to the prior covariance matrix, the Kalman gain matrix and the observation covariance matrix. 5.根据权利要求4所述的基于卡尔曼滤波的无人机定位方法，其特征在于，所述采样点为：5. the UAV positioning method based on Kalman filtering according to claim 4, is characterized in that, described sampling point is:
Figure 925702DEST_PATH_IMAGE001
Figure 925702DEST_PATH_IMAGE001
Figure 731984DEST_PATH_IMAGE002
Figure 731984DEST_PATH_IMAGE002
Figure 643964DEST_PATH_IMAGE003
Figure 643964DEST_PATH_IMAGE003
 式中，
Figure 510289DEST_PATH_IMAGE004
为所述采样点；
Figure 239210DEST_PATH_IMAGE005
为所述先验位置值；
Figure 114763DEST_PATH_IMAGE006
为状态方程的维度；
Figure 612740DEST_PATH_IMAGE007
为所述采样点与所述当前位置值距离的参量；Pk为第k时刻的协方差矩阵。In the formula,
Figure 510289DEST_PATH_IMAGE004
is the sampling point;
Figure 239210DEST_PATH_IMAGE005
is the prior position value;
Figure 114763DEST_PATH_IMAGE006
is the dimension of the state equation;
Figure 612740DEST_PATH_IMAGE007
is the parameter of the distance between the sampling point and the current position value; P k is the covariance matrix at the kth moment. 6.根据权利要求5所述的基于卡尔曼滤波的无人机定位方法，其特征在于，所述先验位置值和所述先验协方差矩阵分别为：6. The unmanned aerial vehicle positioning method based on Kalman filtering according to claim 5, is characterized in that, described priori position value and described priori covariance matrix are respectively:
Figure 649966DEST_PATH_IMAGE008
Figure 649966DEST_PATH_IMAGE008
Figure 131763DEST_PATH_IMAGE009
Figure 131763DEST_PATH_IMAGE009
Figure 279848DEST_PATH_IMAGE010
Figure 279848DEST_PATH_IMAGE010
Figure 897911DEST_PATH_IMAGE011
Figure 897911DEST_PATH_IMAGE011
Figure 371617DEST_PATH_IMAGE012
Figure 371617DEST_PATH_IMAGE012
 式中，
Figure 75131DEST_PATH_IMAGE013
为所述先验位置值；
Figure 292486DEST_PATH_IMAGE014
为所述先验协方差矩阵；
Figure 765056DEST_PATH_IMAGE015
为所述一阶统计权系数；
Figure 675243DEST_PATH_IMAGE016
为所述二阶统计权系数；
Figure 866053DEST_PATH_IMAGE017
为所述一步预测采样点；
Figure 621519DEST_PATH_IMAGE018
为所述采样点的一步预测函数；
Figure 214174DEST_PATH_IMAGE019
为第一白噪声序列；
Figure 498525DEST_PATH_IMAGE020
为第k时刻的当前量测噪声协方差矩阵。In the formula,
Figure 75131DEST_PATH_IMAGE013
is the prior position 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
is the one-step prediction function of the sampling point;
Figure 214174DEST_PATH_IMAGE019
is the first white noise sequence;
Figure 498525DEST_PATH_IMAGE020
is the current measurement noise covariance matrix at the kth time. 7.根据权利要求6所述的基于卡尔曼滤波的无人机定位方法，其特征在于，所述下一滑动窗口长度为：7. the UAV positioning method based on Kalman filtering according to claim 6, is characterized in that, described next sliding window length is:
Figure 176631DEST_PATH_IMAGE021
Figure 176631DEST_PATH_IMAGE021
Figure 470209DEST_PATH_IMAGE022
Figure 470209DEST_PATH_IMAGE022
 式中，
Figure 182950DEST_PATH_IMAGE023
为所述下一滑动窗口长度；
Figure 903782DEST_PATH_IMAGE024
为所述当前滑动窗口长度；L为量测维度；
Figure 803605DEST_PATH_IMAGE025
为所述当前残差序列；
Figure 897944DEST_PATH_IMAGE026
为所述当前残差序列中第j个元素；
Figure 465192DEST_PATH_IMAGE027
为所述历史平均残差序列中第j个元素；
Figure 356924DEST_PATH_IMAGE028
为所述当前观测值；
Figure 744043DEST_PATH_IMAGE029
为所述当前位置值；
Figure 645003DEST_PATH_IMAGE030
为量测位置值与观测位置值之间的映射关系；||表示取整。In the formula,
Figure 182950DEST_PATH_IMAGE023
is the length of the next sliding window;
Figure 903782DEST_PATH_IMAGE024
is the current sliding window length; L is the measurement dimension;
Figure 803605DEST_PATH_IMAGE025
is the current residual sequence;
Figure 897944DEST_PATH_IMAGE026
is the jth element in the current residual sequence;
Figure 465192DEST_PATH_IMAGE027
is the jth element in the historical average residual sequence;
Figure 356924DEST_PATH_IMAGE028
is the current observation value;
Figure 744043DEST_PATH_IMAGE029
is the current position value;
Figure 645003DEST_PATH_IMAGE030
is the mapping relationship between the measured position value and the observed position value; || means rounding. 8.根据权利要求7所述的基于卡尔曼滤波的无人机定位方法，其特征在于，所述后验位置值和所述后验协方差矩阵分别为：8. the UAV positioning method based on Kalman filtering according to claim 7, is characterized in that, described posterior position value and described posterior covariance matrix are respectively:
Figure 66757DEST_PATH_IMAGE031
Figure 66757DEST_PATH_IMAGE031
Figure 129391DEST_PATH_IMAGE032
Figure 129391DEST_PATH_IMAGE032
Figure 269385DEST_PATH_IMAGE033
Figure 269385DEST_PATH_IMAGE033
Figure 442878DEST_PATH_IMAGE034
Figure 442878DEST_PATH_IMAGE034
Figure 719138DEST_PATH_IMAGE035
Figure 719138DEST_PATH_IMAGE035
Figure 952674DEST_PATH_IMAGE036
Figure 952674DEST_PATH_IMAGE036
Figure 314385DEST_PATH_IMAGE037
Figure 314385DEST_PATH_IMAGE037
Figure 557147DEST_PATH_IMAGE038
Figure 557147DEST_PATH_IMAGE038
Figure 687914DEST_PATH_IMAGE039
Figure 687914DEST_PATH_IMAGE039
Figure 92351DEST_PATH_IMAGE040
Figure 92351DEST_PATH_IMAGE040
 式中，
Figure 941358DEST_PATH_IMAGE041
为所述后验位置值；
Figure 987812DEST_PATH_IMAGE042
为所述先验位置值；
Figure 973085DEST_PATH_IMAGE043
为所述卡尔曼增益矩阵；
Figure 548423DEST_PATH_IMAGE044
为所述观测位置值；
Figure 884726DEST_PATH_IMAGE045
为估测位置值；
Figure 203712DEST_PATH_IMAGE046
为所述后验协方差矩阵；
Figure 574651DEST_PATH_IMAGE047
为所述先验协方差矩阵的估计协方差矩阵；
Figure 320890DEST_PATH_IMAGE048
为所述卡尔曼增益矩阵的转置矩阵；
Figure 144489DEST_PATH_IMAGE049
为所述观测协方差矩阵，
Figure 1587DEST_PATH_IMAGE050
为所述量测协方差矩阵；
Figure 961453DEST_PATH_IMAGE051
为第二白噪声序列；
Figure 412681DEST_PATH_IMAGE052
为估计系统噪声协方差矩阵；
Figure 457998DEST_PATH_IMAGE053
为估计量测噪声协方差矩阵。In the formula,
Figure 941358DEST_PATH_IMAGE041
is the posterior position value;
Figure 987812DEST_PATH_IMAGE042
is the prior position value;
Figure 973085DEST_PATH_IMAGE043
is the Kalman gain matrix;
Figure 548423DEST_PATH_IMAGE044
is the observed position value;
Figure 884726DEST_PATH_IMAGE045
is the estimated position value;
Figure 203712DEST_PATH_IMAGE046
is the posterior covariance matrix;
Figure 574651DEST_PATH_IMAGE047
is the estimated covariance matrix of the prior covariance matrix;
Figure 320890DEST_PATH_IMAGE048
is the transpose matrix of the Kalman gain matrix;
Figure 144489DEST_PATH_IMAGE049
is the observation covariance matrix,
Figure 1587DEST_PATH_IMAGE050
is the measurement covariance matrix;
Figure 961453DEST_PATH_IMAGE051
is the second white noise sequence;
Figure 412681DEST_PATH_IMAGE052
is the estimated system noise covariance matrix;
Figure 457998DEST_PATH_IMAGE053
is the estimated measurement noise covariance matrix. 9.根据权利要求8所述的基于卡尔曼滤波的无人机定位方法，其特征在于，所述估计系统噪声协方差矩阵和所述估计量测噪声协方差矩阵分别为：9. The UAV positioning method based on Kalman filtering according to claim 8, wherein the estimated system noise covariance matrix and the estimated measurement noise covariance matrix are respectively:
Figure 384365DEST_PATH_IMAGE054
Figure 384365DEST_PATH_IMAGE054
Figure 464317DEST_PATH_IMAGE055
Figure 464317DEST_PATH_IMAGE055
Figure 552359DEST_PATH_IMAGE056
Figure 552359DEST_PATH_IMAGE056
Figure 350550DEST_PATH_IMAGE057
Figure 350550DEST_PATH_IMAGE057
Figure 815030DEST_PATH_IMAGE058
Figure 815030DEST_PATH_IMAGE058
 式中，
Figure 749488DEST_PATH_IMAGE059
为第k时刻的新息序列；
Figure 274010DEST_PATH_IMAGE060
为第k-i时刻的新息序列；
Figure 293918DEST_PATH_IMAGE061
为k-i时刻的残差序列；
Figure 296510DEST_PATH_IMAGE062
为第k时刻的先验位置值。In the formula,
Figure 749488DEST_PATH_IMAGE059
is the innovation sequence at the kth time;
Figure 274010DEST_PATH_IMAGE060
is the innovation sequence at the kith moment;
Figure 293918DEST_PATH_IMAGE061
is the residual sequence at time ki;
Figure 296510DEST_PATH_IMAGE062
is the prior position value at the kth time. 10.一种基于卡尔曼滤波的无人机定位装置，其特征在于，包括：10. A UAV positioning device based on Kalman filtering, characterized in that, comprising: 采样单元，用于基于预设采样方法对无人机的当前位置值进行采样，获得多个采样点；a sampling unit, used for sampling the current position value of the UAV based on the preset sampling method to obtain multiple sampling points; 先验值确定单元，用于基于所述多个采样点，确定所述无人机下一时刻的先验位置值和先验协方差矩阵；A priori value determination unit, configured to determine, based on the plurality of sampling points, the prior position value and prior covariance matrix of the UAV at the next moment; 窗口长度更新单元，用于根据所述当前时刻的当前滑动窗口长度确定所述下一时刻的下一滑动窗口长度；a window length update unit, configured to determine the next sliding window length at the next moment according to the current sliding window length at the current moment; 噪声协方差矩阵确定单元，用于根据所述下一滑动窗口长度确定所述下一时刻的估计系统噪声协方差矩阵和估计量测噪声协方差矩阵；a noise covariance matrix determining unit, configured to determine the estimated system noise covariance matrix and the estimated measurement noise covariance matrix at the next moment according to the length of the next sliding window; 定位单元，用于获取下一时刻的观测位置值，并根据所述观测位置值、所述估计系统噪声协方差矩阵、所述估计量测噪声协方差矩阵、所述先验位置值以及所述先验协方差矩阵确定所述下一时刻的后验位置值和后验协方差矩阵，以对无人机定位。a positioning unit, configured to obtain the observed position value at the next moment, and based on the observed position value, the estimated system noise covariance matrix, the estimated measurement noise covariance matrix, the prior position value and the The prior covariance matrix determines the posterior position value and the posterior covariance matrix at the next moment to locate the UAV.
CN202110943418.6A 2021-08-17 2021-08-17 UAV positioning method and device based on Kalman filter Active CN113390421B (en)

Priority Applications (1)

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

Applications Claiming Priority (1)

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

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 UAV positioning method and device based on Kalman filter

Country Status (1)

Country Link
CN (1) CN113390421B (en)

Cited By (2)

* 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
CN115903872A (en) * 2022-11-30 2023-04-04 广东汇天航空航天科技有限公司 Flight control method and device, aircraft and computer readable 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 (2)

* 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
CN115903872A (en) * 2022-11-30 2023-04-04 广东汇天航空航天科技有限公司 Flight control method and device, aircraft and computer readable storage medium

Also Published As

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

Similar Documents

Publication Publication Date Title
CN101943758B (en) Position calculation method and position calculation apparatus
US20140341465A1 (en) Real-time pose estimation system using inertial and feature measurements
CN107255795B (en) Indoor mobile robot positioning method and device based on EKF/EFIR hybrid filtering
CN113390421B (en) UAV positioning method and device based on Kalman filter
US9116002B2 (en) Context determination to assist location determination accuracy
CN109283562A (en) A method and device for three-dimensional positioning of vehicles in the Internet of Vehicles
CN115164936B (en) Global pose correction method and device for point cloud stitching in high-precision map production
CN113189541A (en) Positioning method, device and equipment
KR101786280B1 (en) Device, method and media for dynamically calibrating magnetic sensors
CN115711616B (en) Smooth positioning method and device for indoor and outdoor traversing unmanned aerial vehicle
Zhao et al. Robust adaptive heading tracking fusion for polarization compass with uncertain dynamics and external disturbances
Stepanov et al. A procedure of comparative analysis of recursive nonlinear filtering algorithms in navigation data processing based on predictive simulation
Liu et al. Adaptive algorithm of magnetic heading detection
Havyarimana et al. A Hybrid Approach‐Based Sparse Gaussian Kernel Model for Vehicle State Determination during Outage‐Free and Complete‐Outage GPS Periods
JP2017501389A (en) Wireless positioning method and apparatus
CN110207699B (en) Positioning method and device
Atia Design and simulation of sensor fusion using symbolic engines
CN113034570B (en) Image processing method, device and electronic device
Qi et al. Continuous self‐calibration of platform inertial navigation system based on attitude quaternion model
US10309781B2 (en) Computing a magnetic heading
CN115032672A (en) Fusion positioning method and system based on positioning subsystem
CN117405108B (en) Target attitude measurement method, system, electronic equipment and medium
Burkhard et al. Linear Estimation of Deterministic Accelerometer Errors
CN118913252B (en) A UAV navigation method and device
CN116448105B (en) Pose updating method and device, electronic equipment and storage medium

Legal Events

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