CN105136145A - Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method - Google Patents

Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method Download PDF

Info

Publication number
CN105136145A
CN105136145A CN201510489795.1A CN201510489795A CN105136145A CN 105136145 A CN105136145 A CN 105136145A CN 201510489795 A CN201510489795 A CN 201510489795A CN 105136145 A CN105136145 A CN 105136145A
Authority
CN
China
Prior art keywords
formula
unmanned aerial
angle
attitude angle
moment
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201510489795.1A
Other languages
Chinese (zh)
Inventor
陈兴林
罗文嘉
李松
高怡然
徐川川
于志亮
崔宁
杨昱昊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201510489795.1A priority Critical patent/CN105136145A/en
Publication of CN105136145A publication Critical patent/CN105136145A/en
Pending legal-status Critical Current

Links

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

Abstract

The invention relates to the field of multi-sensor data fusion in integrated navigation, in particular to a Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion improved method, and aims to solve the existing problem that due to heavy computation of Kalman filtering in a quadrotor unmanned aerial vehicle attitude computation process, attitude data cannot be acquired in real time. The method provided by the invention improves the Kalman filtering equation set, i.e. improves the current state prediction equation, the current state error covariance prediction equation, the current optimal attitude angle equation, the Kalman gain Kg(k) equation and the covariance equation of the current optimal attitude angle equation, realizes off-line computation of partial data, and greatly reduces the computation amount of a flight control board processor, thereby meeting the real-time requirement and data accuracy requirement of quadrotor unmanned aerial vehicle attitude data. The method provided by the invention can be applied to the technical field of multi-sensor data fusion in integrated navigation.

Description

A kind of method merged based on four rotor wing unmanned aerial vehicle attitude datas of Kalman filtering
Technical field
The present invention relates to Fusion field in integrated navigation, particularly relate to a kind of method merged based on four rotor wing unmanned aerial vehicle attitude datas of Kalman filtering.
Background technology
Kalman filtering is an optimization autoregression data processing algorithm.Be widely used in Computer Image Processing aspect in recent years.In four rotor wing unmanned aerial vehicle flight systems, need carry out comprehensive to the data measured by gyroscope, acceleration transducer and magnetometer sensor and correct.If use simple mean filter, be difficult to meet the requirement in precision and real-time.Therefore merged by Kalman filtering degree of will speed up sensor, magnetometric sensor and gyrostatic data, the interference effect of noise is inhibit well when the real-time attitude of calculating four rotor wing unmanned aerial vehicle aircraft, to improve measuring accuracy, for autonomous flight control creates condition.
But also there is an obvious shortcoming in Kalman filtering in four rotor wing unmanned aerial vehicle attitude calculating processes, also be the Kalman filtering algorithm all Problems existing based on digital platform in fact, have a large amount of matrix multiplications in system of equations in algorithm, the matrix that also has had even is taken advantage of.This can cause the intermediate result occurring that bit wide is very wide, thus is difficult to carry out interative computation.Namely the greatest difficulty faced in Digital Implementation is matrix inversion operation, and when order of matrix number increases, the complexity of its inversion operation increases with geometric series especially.Kalman filtering algorithm is realized by interative computation, and this just requires that, in each interative computation, Kalman filtering algorithm system of equations all must be rerun once, and this will make CPU bear complexity and extremely heavy computing.According to above-mentioned analysis, can find out that application Kalman filtering algorithm carries out attitude data fusion, operand is very large, requires very high to the processor chips flying to control plate, just seems very necessary so carry out simplifying for the Kalman filtering algorithm being applied in four rotor wing unmanned aerial vehicle attitude datas fusions.
Summary of the invention
The present invention is that to solve existing Kalman filtering operand in four rotor wing unmanned aerial vehicle attitude calculating processes too large and cannot the problem of Real-time Obtaining attitude data, and proposes a kind of method merged based on four rotor wing unmanned aerial vehicle attitude datas of Kalman filtering.
Theoretical foundation based on four rotor wing unmanned aerial vehicle attitude data fusion methods of Kalman filtering:
In four rotor wing unmanned aerial vehicle attitude algorithms, gyroscope can provide the dynamic angle change of moment, ensure stability when carrier has a dynamic acceleration, but the impact of inherent characteristic, temperature and integral process by itself, cumulative errors can be produced along with the prolongation of working time; Acceleration transducer and magnetometric sensor can provide static angular exactly, but are easily subject to noise during motion.So in four rotor wing unmanned aerial vehicle attitude algorithm processes, the data of comprehensively gyroscope, acceleration transducer and magnetometric sensor the attitude of four rotor wing unmanned aerial vehicles should be resolved.The data of three sensors can be merged well by Kalman filtering algorithm.
As far as possible the basic ideas of the four rotor wing unmanned aerial vehicle attitude data fusion methods based on Kalman filtering described in the application reduce matrix multiplication operation; Avoid the inversion operation of matrix; Simplify iterative process.
Based on the method that four rotor wing unmanned aerial vehicle attitude datas of Kalman filtering merge, carry out according to the following steps:
One, according to the characteristic of four rotor wing unmanned aerial vehicle attitude-measuring sensors, select with the actual attitude angle θ of four rotor wing unmanned aerial vehicles and gyrostatic measuring error σ for state vector, the attitude angle θ recorded with acceleration transducer and magnetometric sensor surveyfor observed reading, obtain the four corresponding state equation of rotor wing unmanned aerial vehicle attitude algorithm system and observation equations, as shown in formula (1),
Wherein be the differential of the actual attitude angle θ of four rotor wing unmanned aerial vehicles, for the differential of gyrostatic measuring error σ, ω is the angular velocity that gyroscope exports, ω in vainfor gyrostatic measurement white noise, v in vainfor the measurement white noise of acceleration transducer and magnetometric sensor, ω in vainwith v in vainseparate;
Two, the treatable discretize signal of computing machine is turned to by discrete for the continuous signal resolving system, the sampling period that system is resolved in order is Ts, obtain state equation X (k) of discretize and measure equation V (k), as shown in formula (2)
The wherein angular velocity that exports for k-1 moment gyroscope of ω (k-1); The roll angle recorded in the k moment with acceleration transducer and magnetometric sensor, the angle of pitch, crab angle are for measuring attitude angle, v in vaink () measures the white noise deviation of attitude angle for this reason, covariance corresponding to this white noise deviation is measurement noises covariance R;
Three, on the basis of formula (2), Kalman filter equation group (3) ~ (7) are built, wherein current state predictive equation, as shown in formula (3),
X(k|k-1)＝AX(k-1|k-1)+Bω(k-1)，(3)
Wherein A and B is for resolving systematic parameter, A = 1 - T s 0 1 , B = T s 0 , X (k-1|k-1) is the attitude angle of k-1 moment optimum, the controlled quentity controlled variable that ω (k-1) is the k-1 moment, the i.e. angular velocity of gyroscope output, X (k|k-1) is the unmanned plane current pose angle predicted according to X (k-1|k-1);
Current state error covariance predictive equation as shown in formula (4),
P(k|k-1)＝AP(k-1|k-1)A T+Q，(4)
Wherein P (k-1|k-1) is the covariance that X (k-1|k-1) is corresponding, P (k|k-1) is the covariance that X (k|k-1) is corresponding, the optimum attitude angle sum of the attitude angle deviation and k-1 moment that calculate the k moment with gyroscope is for current predictive attitude angle, the noise bias of this current predictive attitude angle is G (k), and the covariance that this noise bias is corresponding is process noise covariance Q;
By k moment attitude angle predicted value and measured value with kalman gain K gk () merges for ratio, just can obtain optimum attitude angle X (k|k) in k moment, as shown in formula (5),
X(k|k)＝X(k|k-1)+K g(k)(V(k)-HX(k|k-1))，(5)
Wherein H is measuring system parameter, H=[10], K gk () is kalman gain, V (k) is that acceleration transducer and magnetometric sensor obtain the measured value of attitude angle in the k moment, and X (k|k-1) is the predicted value of k moment attitude angle;
K in formula (5) g(k), namely kalman gain is tried to achieve by following formula, as shown in formula (6),
K g ( k ) = P ( k | k - 1 ) H T ( H P ( k | k - 1 ) H T + R ) , - - - ( 6 )
Wherein P (k|k-1) is the covariance of X (k|k-1) correspondence, and the physical significance of R is shown in the explanation in step 2;
The covariance P (k|k) at optimum attitude angle X (k|k) in k moment as shown in formula (7),
P(k|k)＝(I-K g(k)H)P(k|k-1)，(7)
Wherein, I is second order unit matrix, and P (k|k-1) is the covariance that X (k|k-1) is corresponding;
Four, carry out mathematics manipulation to Kalman filter equation group (3) ~ (7), Kalman filter equation group (8) ~ (12) after being improved, detailed process is as follows:
Merge formula (4) and (7) and formula (8) can be obtained:
P(k|k-1)＝A((I-K g(k-1)H)P(k-1|k-2))A T+Q，(8)
Formula (9) can be obtained according to formula (6):
K g ( k - 1 ) = P ( k - 1 | k - 2 ) H T ( H P ( k - 1 | k - 2 ) H T + R ) , - - - ( 9 )
Formula (5) is out of shape, formula (10) can be obtained:
X(k|k)＝(I-K g(k)H)X(k|k-1)+K g(k)V(k)，(10)
Formula (11) can be obtained according to formula (3):
X(k|k)＝(I-K g(k)H)(AX(k-1|k-1)+Bω(k))+K g(k)V(k)
＝(I-K g(k)H)AX(k-1|k-1)+(I-K g(k)H)Bω(k)+K g(k)V(k)，(11)
By (the I-K in formula (11) g(k) H) A and (I-K g(k) H) B, replace with respectively as E a(k) and E bk (), substitutes into formula (11) and obtains formula (12):
X(k|k)＝E A(k)X(k-1|k-1)+E B(k)ω(K)+K g(k)Z(k)，(12)
Five, to the E in formula (12) a(k), E b(k) and K gk () first evaluation, uses E in each iterative process a(k), E b(k) and K gk, time (), directly flying to call in control sheet processor chip, continuous double counting formula (8) ~ (10) and (12), until find the optimum attitude angle in each moment.
The present invention includes following beneficial effect:
1, operation time is reduced, thus requirement of real time: the four rotor wing unmanned aerial vehicle attitude data blending algorithms based on Kalman filtering adopting standard, because operand is large, cause the processing time oversize, Real-time Obtaining four rotor attitude data cannot be realized in engineering reality at all, and adopt the application's innovatory algorithm, partial data calculated off-line, greatly reduce the calculated amount flying to control sheet processor, control sheet processor STM32F103ZE is flown for conventional, the acquisition completing whole four rotor wing unmanned aerial vehicle attitude datas is less than 1 millisecond, requirement of real time;
2, error is reduced, to meet data precision requirement: the four rotor wing unmanned aerial vehicle attitude data blending algorithms based on Kalman filtering adopting standard, the restriction being subject to processing device chip bit wide in Karman equation interative computation must constantly data intercept, thus cause the loss of data precision, the error of last attitude accuracy is caused to be greater than 15%, the requirement of data precision cannot be reached, and adopt innovatory algorithm, E a(k), E b(k) and K gk the calculated off-line of () is not by flying the restriction controlling sheet processor chip bit wide, the error of last attitude accuracy is 2% ~ 5%, meets data precision requirement.
Accompanying drawing explanation
Fig. 1 is the four rotor wing unmanned aerial vehicle attitude data blending algorithm block diagrams based on Kalman filtering,
In figure, accelerometer is acceleration transducer, and magnetometer is magnetometric sensor.
Embodiment
For enabling above-mentioned purpose of the present invention, feature and advantage become apparent more, and below in conjunction with Fig. 1 and embodiment, the present invention is further detailed explanation.
A kind of method merged based on four rotor wing unmanned aerial vehicle attitude datas of Kalman filtering described in embodiment one, present embodiment, carry out according to the following steps:
One, according to the characteristic of four rotor wing unmanned aerial vehicle attitude-measuring sensors, select with the actual attitude angle θ of four rotor wing unmanned aerial vehicles and gyrostatic measuring error σ for state vector, the attitude angle θ recorded with acceleration transducer and magnetometric sensor surveyfor observed reading, obtain the four corresponding state equation of rotor wing unmanned aerial vehicle attitude algorithm system and observation equations, as shown in formula (1),
Wherein be the differential of the actual attitude angle θ of four rotor wing unmanned aerial vehicles, for the differential of gyrostatic measuring error σ, ω is the angular velocity that gyroscope exports, ω in vainfor gyrostatic measurement white noise, v in vainfor the measurement white noise of acceleration transducer and magnetometric sensor, ω in vainwith v in vainseparate;
Two, the treatable discretize signal of computing machine is turned to by discrete for the continuous signal resolving system, the sampling period that system is resolved in order is Ts, obtain state equation X (k) of discretize and measure equation V (k), as shown in formula (2)
The wherein angular velocity that exports for k-1 moment gyroscope of ω (k-1); The roll angle recorded in the k moment with acceleration transducer and magnetometric sensor, the angle of pitch, crab angle are for measuring attitude angle, v in vaink () measures the white noise deviation of attitude angle for this reason, covariance corresponding to this white noise deviation is measurement noises covariance R;
Three, on the basis of formula (2), Kalman filter equation group (3) ~ (7) are built, wherein current state predictive equation, as shown in formula (3),
X(k|k-1)＝AX(k-1|k-1)+Bω(k-1)，(3)
Wherein A and B is for resolving systematic parameter, A = 1 - T s 0 1 , B = T s 0 , X (k-1|k-1) is the attitude angle of k-1 moment optimum, the controlled quentity controlled variable that ω (k-1) is the k-1 moment, the i.e. angular velocity of gyroscope output, X (k|k-1) is the unmanned plane current pose angle predicted according to X (k-1|k-1);
Current state error covariance predictive equation as shown in formula (4),
P(k|k-1)＝AP(k-1|k-1)A T+Q，(4)
Wherein P (k-1|k-1) is the covariance that X (k-1|k-1) is corresponding, P (k|k-1) is the covariance that X (k|k-1) is corresponding, the optimum attitude angle sum of the attitude angle deviation and k-1 moment that calculate the k moment with gyroscope is for current predictive attitude angle, the noise bias of this current predictive attitude angle is G (k), and the covariance that this noise bias is corresponding is process noise covariance Q;
By k moment attitude angle predicted value and measured value with kalman gain K gk () merges for ratio, just can obtain optimum attitude angle X (k|k) in k moment, as shown in formula (5),
X(k|k)＝X(k|k-1)+K g(k)(V(k)-HX(k|k-1))，(5)
Wherein H is measuring system parameter, H=[10], K gk () is kalman gain, V (k) is that acceleration transducer and magnetometric sensor obtain the measured value of attitude angle in the k moment, and X (k|k-1) is the predicted value of k moment attitude angle;
K in formula (5) g(k), namely kalman gain is tried to achieve by following formula, as shown in formula (6),
K g ( k ) = P ( k | k - 1 ) H T ( H P ( k | k - 1 ) H T + R ) , - - - ( 6 )
Wherein P (k|k-1) is the covariance of X (k|k-1) correspondence, and the physical significance of R is shown in the explanation in step 2;
The covariance P (k|k) at optimum attitude angle X (k|k) in k moment as shown in formula (7),
P(k|k)＝(I-K g(k)H)P(k|k-1)，(7)
Wherein, I is second order unit matrix, and P (k|k-1) is the covariance that X (k|k-1) is corresponding;
Formula (3) ~ formula (7) forms Kalman filtering algorithm system of equations, wherein formula (5) ~ (7) are the state updating system of equations of Kalman filter, the Posterior estimator in time calculating, and as the prior estimate of recursive operation next time; For the autoregression realizing multi-sensor information fusion Karman equation upgrades, estimation of error Q and R need be constantly updated, are gone round and begun again in formula (3) ~ (7), circulate computing repeatedly, until find the optimum attitude angle in each moment;
Four, observe Kalman filtering algorithm system of equations, comprising two iterative process, first iterative process is represented by formula (5), and object is the value of the estimation obtaining each iteration; Another iterative process is made up of formula (4), (6) and (7), and object is in each iterative process, for filtering estimate equation obtains corresponding kalman gain K gk (), merges formula (4) and (7) and can obtain formula (8):
P(k|k-1)＝A((I-K g(k-1)H)P(k-1|k-2))A T+Q，(8)
The merging of formula (4) and (7), what complete is the iteration of P (k|k-1);
Can obtain according to formula (6):
K g ( k - 1 ) = P ( k - 1 | k - 2 ) H T ( H P ( k - 1 | k - 2 ) H T + R ) , - - - ( 9 )
Compared by formula (6) and (9), the iteration of what known formula (8) in fact completed is kalman gain;
The intermediate variable of P (k|k) and P (k|k-1) just kalman gain iteration, and matrix A and H are known matrix, utilizes A and H can carry out the iterative process of kalman gain by off-line, obtains the kalman gain K of each interative computation gk (), uses for filtering estimate equation; The reason that can process like this is except H is relevant with sample frequency in these three equations, all the other are constant, and in actual four rotor attitude algorithm systems, its sample frequency is determined, therefore can regard H as constant matrices, like this, Kalman filtering algorithm just can be reduced to an equation, i.e. formula (5), is out of shape formula (5), can obtains:
X(k|k)＝(I-K g(k)H)X(k|k-1)+K g(k)V(k)，(10)
Can obtain according to formula (3):
X(k|k)＝(I-K g(k)H)(AX(k-1|k-1)+Bω(k))+K g(k)V(k)
＝(I-K g(k)H)AX(k-1|k-1)+(I-K g(k)H)Bω(k)+K g(k)V(k)，(11)
The wherein angular velocity that exports for k moment gyroscope of ω (k);
Due to first K can be tried to achieve in iterative process gk (), so in each iterative process, can be regarded as constant, therefore, and (the I-K in formula (11) g(k) H) A and (I-K g(k) H) B also can first try to achieve, and it is set to E respectively a(k) and E bk (), substitutes into formula public affairs (11) and obtains:
X(k|k)＝E A(k)X(k-1|k-1)+E B(k)ω(K)+K g(k)Z(k)，(12)
Five, to the E in formula (12) a(k), E b(k) and K gk () first evaluation, uses E in each iterative process a(k), E b(k) and K gtime (k), directly flying to call in control sheet processor chip, continuous double counting formula (8) ~ (10) and (12) process, until find the optimum attitude angle in each moment, thus make to fly control sheet processor chip calculated amount and greatly simplified.
The present invention includes following beneficial effect:
1, operation time is reduced, thus requirement of real time: the four rotor wing unmanned aerial vehicle attitude data blending algorithms based on Kalman filtering adopting standard, because operand is large, cause the processing time oversize, Real-time Obtaining four rotor attitude data cannot be realized in engineering reality at all, and adopt the application's innovatory algorithm, partial data calculated off-line, greatly reduce the calculated amount flying to control sheet processor, control sheet processor STM32F103ZE is flown for conventional, the acquisition completing whole four rotor wing unmanned aerial vehicle attitude datas is less than 1 millisecond, requirement of real time;
2, error is reduced, to meet data precision requirement: the four rotor wing unmanned aerial vehicle attitude data blending algorithms based on Kalman filtering adopting standard, the restriction being subject to processing device chip bit wide in Karman equation interative computation must constantly data intercept, thus cause the loss of data precision, the error of last attitude accuracy is caused to be greater than 15%, the requirement of data precision cannot be reached, and adopt innovatory algorithm, E a(k), E b(k) and K gk the calculated off-line of () is not by flying the restriction controlling sheet processor chip bit wide, the error of data precision is 2% ~ 5%, meets data precision requirement.
Embodiment two, present embodiment are that actual attitude angle θ described in step one is the vectorial angle of the actual roll angle of four rotor wing unmanned aerial vehicles, the angle of pitch and crab angle formation to a kind of the further illustrating of method of merging based on four rotor wing unmanned aerial vehicle attitude datas of Kalman filtering described in embodiment one.
Actual attitude angle θ directly cannot be recorded by surveying instrument, by the roll angle of actual measurement unmanned plane, the angle of pitch and crab angle, and can only represent actual attitude angle θ by the form of vector.
Embodiment three, present embodiment are to a kind of the further illustrating of method of merging based on four rotor wing unmanned aerial vehicle attitude datas of Kalman filtering described in embodiment one, E described in step 13 a(k), E b(k) and K gk the evaluation of () adopts offline mode, and result of evaluation be stored in and fly in control sheet processor chip.
E a(k), E b(k) and K gk the evaluation of () adopts offline mode, reduce the calculated amount flying to control sheet processor widely.

Claims (3)

1., based on the method that four rotor wing unmanned aerial vehicle attitude datas of Kalman filtering merge, it is characterized in that it is realized by following steps:
One, according to the characteristic of four rotor wing unmanned aerial vehicle attitude-measuring sensors, select with the actual attitude angle θ of four rotor wing unmanned aerial vehicles and gyrostatic measuring error σ for state vector, the attitude angle θ recorded with acceleration transducer and magnetometric sensor surveyfor observed reading, obtain the four corresponding state equation of rotor wing unmanned aerial vehicle attitude algorithm system and observation equations, as shown in formula (1),
Wherein be the differential of the actual attitude angle θ of four rotor wing unmanned aerial vehicles, for the differential of gyrostatic measuring error σ, ω is the angular velocity that gyroscope exports, ω in vainfor gyrostatic measurement white noise, v in vainfor the measurement white noise of acceleration transducer and magnetometric sensor, ω in vainwith v in vainseparate;
Two, the treatable discretize signal of computing machine is turned to by discrete for the continuous signal resolving system, the sampling period that system is resolved in order is Ts, obtain state equation X (k) of discretize and measure equation V (k), as shown in formula (2)
The wherein angular velocity that exports for k-1 moment gyroscope of ω (k-1); The roll angle recorded in the k moment with acceleration transducer and magnetometric sensor, the angle of pitch, crab angle are for measuring attitude angle, v in vaink () measures the white noise deviation of attitude angle for this reason, covariance corresponding to this white noise deviation is measurement noises covariance R;
Three, on the basis of formula (2), Kalman filter equation group (3) ~ (7) are built, wherein current state predictive equation, as shown in formula (3),
X(k|k-1)＝AX(k-1|k-1)+Bω(k-1)，(3)
Wherein A and B is for resolving systematic parameter, A = 1 - T s 0 1 , B = T s 0 , X (k -1|k -1) be the attitude angle of k-1 moment optimum, the controlled quentity controlled variable that ω (k-1) is the k-1 moment, i.e. the angular velocity that exports of gyroscope, X (k|k-1) is the unmanned plane current pose angle predicted according to X (k-1|k-1);
Current state error covariance predictive equation as shown in formula (4),
P(k|k-1)＝AP(k-1|k-1)A T+Q，(4)
Wherein P (k-1|k-1) is the covariance that X (k-1|k-1) is corresponding, P (k|k-1) is the covariance that X (k|k-1) is corresponding, the optimum attitude angle sum of the attitude angle deviation and k-1 moment that calculate the k moment with gyroscope is for current predictive attitude angle, the noise bias of this current predictive attitude angle is G (k), and the covariance that this noise bias is corresponding is process noise covariance Q;
By k moment attitude angle predicted value and measured value with kalman gain K gk () merges for ratio, just can obtain optimum attitude angle X (k|k) in k moment, as shown in formula (5),
X(k|k)＝X(k|k-1)+K g(k)(V(k)-HX(k|k-1))，(5)
Wherein H is measuring system parameter, H=[10], K gk () is kalman gain, V (k) is that acceleration transducer and magnetometric sensor obtain the measured value of attitude angle in the k moment, and X (k|k-1) is the predicted value of k moment attitude angle;
K in formula (5) g(k), namely kalman gain is tried to achieve by following formula, as shown in formula (6),
K g ( k ) = P ( k | k - 1 ) H T ( H P ( k | k - 1 ) H T + R ) , - - - ( 6 )
Wherein P (k|k-1) is the covariance of X (k|k-1) correspondence, and the physical significance of R is shown in the explanation in step 2;
The covariance P (k|k) at optimum attitude angle X (k|k) in k moment as shown in formula (7),
P(k|k)＝(I-K g(k)H)P(k|k-1)，(7)
Wherein, I is second order unit matrix, and P (k|k-1) is the covariance that X (k|k-1) is corresponding;
Four, carry out mathematics manipulation to Kalman filter equation group (3) ~ (7), Kalman filter equation group (8) ~ (12) after being improved, detailed process is as follows:
Merge formula (4) and (7) and formula (8) can be obtained:
P(k|k-1)＝A((I-K g(k-1)H)P(k-1|k-2))A T+Q，(8)
Formula (9) can be obtained according to formula (6):
K g ( k - 1 ) = P ( k - 1 | k - 2 ) H T ( H P ( k - 1 | k - 2 ) H T + R ) , - - - ( 9 )
Formula (5) is out of shape, formula (10) can be obtained:
X(k|k)＝(I-K g(k)H)X(k|k-1)+K g(k)V(k)，(10)
Formula (11) can be obtained according to formula (3):
X(k|k)＝(I-K g(k)H)(AX(k-1|k-1)+Bω(k))+K g(k)V(k)
，(11)
＝(I-K g(k)H)AX(k-1|k-1)+(I-K g(k)H)Bω(k)+K g(k)V(k)
By (the I-K in formula (11) g(k) H) A and (I-K g(k) H) B, replace with respectively as E a(k) and E bk (), substitutes into formula (11) and obtains formula (12):
X(k|k)＝E A(k)X(k-1|k-1)+E B(k)ω(K)+K g(k)Z(k)，(12)
Five, to the E in formula (12) a(k), E b(k) and K gk () first evaluation, uses E in each iterative process a(k), E b(k) and K gk, time (), directly flying to call in control sheet processor chip, continuous double counting formula (8) ~ (10) and (12), until find the optimum attitude angle in each moment.
2. the method for a kind of four rotor wing unmanned aerial vehicle attitude datas fusions based on Kalman filtering as claimed in claim 1, is characterized in that actual attitude angle θ described in step one is the vectorial angle that the actual roll angle of four rotor wing unmanned aerial vehicles, the angle of pitch and crab angle are formed.
3. the method for a kind of four rotor wing unmanned aerial vehicle attitude datas fusions based on Kalman filtering as claimed in claim 1 or 2, is characterized in that E described in step 13 a(k), E b(k) and K gk the evaluation of () adopts offline mode, and result of evaluation be stored in and fly in control sheet processor chip.
CN201510489795.1A 2015-08-11 2015-08-11 Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method Pending CN105136145A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510489795.1A CN105136145A (en) 2015-08-11 2015-08-11 Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510489795.1A CN105136145A (en) 2015-08-11 2015-08-11 Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method

Publications (1)

Publication Number Publication Date
CN105136145A true CN105136145A (en) 2015-12-09

Family

ID=54721585

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510489795.1A Pending CN105136145A (en) 2015-08-11 2015-08-11 Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method

Country Status (1)

Country Link
CN (1) CN105136145A (en)

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105651242A (en) * 2016-04-05 2016-06-08 清华大学深圳研究生院 Method for calculating fusion attitude angle based on complementary Kalman filtering algorithm
CN106021194A (en) * 2016-05-19 2016-10-12 哈尔滨工业大学 Multi-sensor multi-target tracking error estimation method
CN106482734A (en) * 2016-09-28 2017-03-08 湖南优象科技有限公司 A kind of filtering method for IMU Fusion
CN106989745A (en) * 2017-05-31 2017-07-28 合肥工业大学 The fusion method of inclinator and fibre optic gyroscope in push pipe attitude measurement system
CN107783545A (en) * 2016-08-25 2018-03-09 大连楼兰科技股份有限公司 Post disaster relief rotor wing unmanned aerial vehicle obstacle avoidance system based on OODA ring multi-sensor information fusions
CN107783549A (en) * 2016-08-25 2018-03-09 大连楼兰科技股份有限公司 Single rotor plant protection unmanned plane obstacle avoidance system based on multi-sensor information fusion technology
CN107783546A (en) * 2016-08-25 2018-03-09 大连楼兰科技股份有限公司 The plant protection unmanned plane obstacle avoidance system and method for single rotor
CN108230371A (en) * 2017-12-29 2018-06-29 厦门市美亚柏科信息股份有限公司 Tracking target velocity Forecasting Methodology and storage medium based on holder
CN108230370A (en) * 2017-12-29 2018-06-29 厦门市美亚柏科信息股份有限公司 Tracking target velocity Forecasting Methodology and storage medium based on holder
CN108387205A (en) * 2018-01-20 2018-08-10 西安石油大学 The measurement method of drilling tool attitude measurement system based on Fusion
CN108732932A (en) * 2018-06-01 2018-11-02 荷塘智能科技(固安)有限公司 A kind of quadrotor drone Accurate Position Control method based on minimum variance regulator
CN108897333A (en) * 2018-07-06 2018-11-27 深圳臻迪信息技术有限公司 Posture evaluation method, device and unmanned plane
CN109101032A (en) * 2017-06-21 2018-12-28 卡特彼勒公司 For merging the system and method to control machine posture using sensor
CN109144081A (en) * 2017-06-16 2019-01-04 袁兵 Unmanned plane during flying attitude control method based on Kalman filtering
CN109470613A (en) * 2018-11-12 2019-03-15 湖南电气职业技术学院 A kind of unmanned plane PM2.5 detection device based on complementary filter posture blending algorithm
CN109709804A (en) * 2018-12-20 2019-05-03 安徽优思天成智能科技有限公司 A kind of attitude detecting method of servomechanism
CN110566716A (en) * 2019-08-27 2019-12-13 北京无线电计量测试研究所 Kalman filter-based rail valve position measuring system and method
CN112066984A (en) * 2020-09-17 2020-12-11 深圳维特智能科技有限公司 Attitude angle resolving method and device, processing equipment and storage medium
CN112595317A (en) * 2020-10-26 2021-04-02 一飞(海南)科技有限公司 Unmanned aerial vehicle takeoff control method, system, medium, computer equipment and unmanned aerial vehicle
CN113237478A (en) * 2021-05-27 2021-08-10 哈尔滨工业大学 Unmanned aerial vehicle attitude and position estimation method and unmanned aerial vehicle

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002090173A (en) * 2000-09-18 2002-03-27 Toshiba Corp Inertia navigation system and its initial alignment method
US7805244B2 (en) * 2006-05-30 2010-09-28 Inha University Attitude correction apparatus and method for inertial navigation system using camera-type solar sensor
CN102944888A (en) * 2012-11-23 2013-02-27 江苏东大集成电路系统工程技术有限公司 Low calculating quantity global position system (GPS) positioning method based on second-order extended Kalman
CN102955477A (en) * 2012-10-26 2013-03-06 南京信息工程大学 Attitude control system and control method of four-rotor aircraft
CN103246203A (en) * 2013-04-23 2013-08-14 东南大学 GPS (Global Position System) based prediction method for speed state of micro quad-rotor unmanned aerial vehicle
CN104408315A (en) * 2014-11-28 2015-03-11 北京航空航天大学 SINS/GPS integrated navigation based Kalman filter numerical optimization method

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002090173A (en) * 2000-09-18 2002-03-27 Toshiba Corp Inertia navigation system and its initial alignment method
US7805244B2 (en) * 2006-05-30 2010-09-28 Inha University Attitude correction apparatus and method for inertial navigation system using camera-type solar sensor
CN102955477A (en) * 2012-10-26 2013-03-06 南京信息工程大学 Attitude control system and control method of four-rotor aircraft
CN102944888A (en) * 2012-11-23 2013-02-27 江苏东大集成电路系统工程技术有限公司 Low calculating quantity global position system (GPS) positioning method based on second-order extended Kalman
CN103246203A (en) * 2013-04-23 2013-08-14 东南大学 GPS (Global Position System) based prediction method for speed state of micro quad-rotor unmanned aerial vehicle
CN104408315A (en) * 2014-11-28 2015-03-11 北京航空航天大学 SINS/GPS integrated navigation based Kalman filter numerical optimization method

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
万晓凤等: "基于多传感器数据融合的四旋翼飞行器的姿态解算", 《科技导报》 *
汪绍华，杨莹: "基于卡尔曼滤波的四旋翼飞行器姿态估计和控制算法研究", 《控制理论与应用》 *
阮建国，陈炯: "一种适于快速实现的卡尔曼滤波算法的改进", 《电测与仪表》 *

Cited By (30)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105651242A (en) * 2016-04-05 2016-06-08 清华大学深圳研究生院 Method for calculating fusion attitude angle based on complementary Kalman filtering algorithm
CN105651242B (en) * 2016-04-05 2018-08-24 清华大学深圳研究生院 A method of fusion attitude angle is calculated based on complementary Kalman filtering algorithm
CN106021194B (en) * 2016-05-19 2017-10-03 哈尔滨工业大学 A kind of multi-sensor multi-target tracking bias estimation method
CN106021194A (en) * 2016-05-19 2016-10-12 哈尔滨工业大学 Multi-sensor multi-target tracking error estimation method
CN107783549B (en) * 2016-08-25 2020-12-08 大连楼兰科技股份有限公司 Single-rotor-wing plant protection unmanned aerial vehicle obstacle avoidance system based on multi-sensor information fusion technology
CN107783549A (en) * 2016-08-25 2018-03-09 大连楼兰科技股份有限公司 Single rotor plant protection unmanned plane obstacle avoidance system based on multi-sensor information fusion technology
CN107783546A (en) * 2016-08-25 2018-03-09 大连楼兰科技股份有限公司 The plant protection unmanned plane obstacle avoidance system and method for single rotor
CN107783545B (en) * 2016-08-25 2021-04-27 大连楼兰科技股份有限公司 Obstacle avoidance system of post-disaster rescue rotor unmanned aerial vehicle based on OODA (object oriented data acquisition) ring multi-sensor information fusion
CN107783545A (en) * 2016-08-25 2018-03-09 大连楼兰科技股份有限公司 Post disaster relief rotor wing unmanned aerial vehicle obstacle avoidance system based on OODA ring multi-sensor information fusions
CN106482734A (en) * 2016-09-28 2017-03-08 湖南优象科技有限公司 A kind of filtering method for IMU Fusion
CN106989745A (en) * 2017-05-31 2017-07-28 合肥工业大学 The fusion method of inclinator and fibre optic gyroscope in push pipe attitude measurement system
CN109144081A (en) * 2017-06-16 2019-01-04 袁兵 Unmanned plane during flying attitude control method based on Kalman filtering
CN109101032B (en) * 2017-06-21 2023-08-15 卡特彼勒公司 System and method for controlling machine pose using sensor fusion
CN109101032A (en) * 2017-06-21 2018-12-28 卡特彼勒公司 For merging the system and method to control machine posture using sensor
CN108230370B (en) * 2017-12-29 2020-08-04 厦门市美亚柏科信息股份有限公司 Tracking target speed prediction method based on holder and storage medium
CN108230371A (en) * 2017-12-29 2018-06-29 厦门市美亚柏科信息股份有限公司 Tracking target velocity Forecasting Methodology and storage medium based on holder
CN108230370A (en) * 2017-12-29 2018-06-29 厦门市美亚柏科信息股份有限公司 Tracking target velocity Forecasting Methodology and storage medium based on holder
CN108387205B (en) * 2018-01-20 2021-01-01 西安石油大学 Measuring method of drilling tool attitude measuring system based on multi-sensor data fusion
CN108387205A (en) * 2018-01-20 2018-08-10 西安石油大学 The measurement method of drilling tool attitude measurement system based on Fusion
CN108732932A (en) * 2018-06-01 2018-11-02 荷塘智能科技(固安)有限公司 A kind of quadrotor drone Accurate Position Control method based on minimum variance regulator
CN108732932B (en) * 2018-06-01 2021-06-08 荷塘智能科技(固安)有限公司 Four-rotor unmanned aerial vehicle accurate position control method based on minimum variance regulator
CN108897333A (en) * 2018-07-06 2018-11-27 深圳臻迪信息技术有限公司 Posture evaluation method, device and unmanned plane
CN109470613B (en) * 2018-11-12 2020-07-03 湖南电气职业技术学院 Unmanned aerial vehicle PM2.5 detection device based on complementary filtering attitude fusion algorithm
CN109470613A (en) * 2018-11-12 2019-03-15 湖南电气职业技术学院 A kind of unmanned plane PM2.5 detection device based on complementary filter posture blending algorithm
CN109709804A (en) * 2018-12-20 2019-05-03 安徽优思天成智能科技有限公司 A kind of attitude detecting method of servomechanism
CN110566716A (en) * 2019-08-27 2019-12-13 北京无线电计量测试研究所 Kalman filter-based rail valve position measuring system and method
CN112066984A (en) * 2020-09-17 2020-12-11 深圳维特智能科技有限公司 Attitude angle resolving method and device, processing equipment and storage medium
CN112595317A (en) * 2020-10-26 2021-04-02 一飞(海南)科技有限公司 Unmanned aerial vehicle takeoff control method, system, medium, computer equipment and unmanned aerial vehicle
CN113237478A (en) * 2021-05-27 2021-08-10 哈尔滨工业大学 Unmanned aerial vehicle attitude and position estimation method and unmanned aerial vehicle
CN113237478B (en) * 2021-05-27 2022-10-14 哈尔滨工业大学 Unmanned aerial vehicle attitude and position estimation method and unmanned aerial vehicle

Similar Documents

Publication Publication Date Title
CN105136145A (en) Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method
CN103217175B (en) A kind of self-adaptation volume kalman filter method
CN102297693B (en) Method for measuring position and azimuths of object
CN104898681A (en) Tetra-rotor aircraft attitude obtaining method by use of three-order approximation Picard quaternion
CN105785999A (en) Unmanned surface vehicle course motion control method
CN107063245B (en) SINS/DVL combined navigation filtering method based on 5-order SSRCKF
CN105190237A (en) Heading confidence interval estimation
CN108761512A (en) A kind of adaptive CKF filtering methods of missile-borne BDS/SINS deep combinations
CN103822633A (en) Low-cost attitude estimation method based on second-order measurement update
CN103900574A (en) Attitude estimation method based on iteration volume Kalman filter
CN105043388A (en) Vector search iterative matching method based on inertia/gravity matching integrated navigation
CN109507706B (en) GPS signal loss prediction positioning method
CN103604430A (en) Marginalized cubature Kalman filter (CKF)-based gravity aided navigation method
CN111189442B (en) CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method
CN112146655A (en) Elastic model design method for BeiDou/SINS tight integrated navigation system
CN111189454A (en) Unmanned vehicle SLAM navigation method based on rank Kalman filtering
CN111623779A (en) Time-varying system adaptive cascade filtering method suitable for unknown noise characteristics
CN110209180A (en) A kind of UAV navigation method for tracking target based on HuberM-Cubature Kalman filtering
CN111750865A (en) Self-adaptive filtering navigation method for dual-function deep sea unmanned submersible vehicle navigation system
CN111190207A (en) Unmanned aerial vehicle INS BDS integrated navigation method based on PSTCSDREF algorithm
RU2661446C1 (en) Method of determining navigation parameters of object and strapdown inertial navigation system for implementation of method
CN111982126B (en) Design method of full-source BeiDou/SINS elastic state observer model
CN111578931B (en) High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation
CN114111840B (en) DVL error parameter online calibration method based on integrated navigation
Chen et al. A novel hybrid observation prediction methodology for bridging GNSS outages in INS/GNSS systems

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20151209

WD01 Invention patent application deemed withdrawn after publication