Research Article Optical Flow Sensor/INS/Magnetometer...

Research ArticleOptical Flow SensorINSMagnetometer IntegratedNavigation System for MAV in GPS-Denied Environment

Chong Shen1 Zesen Bai2 Huiliang Cao1 Ke Xu1 Chenguang Wang1 Huaiyu Zhang1

Ding Wang1 Jun Tang1 and Jun Liu1

1National Key Laboratory for Electronic Measurement TechnologyKey Laboratory of Instrumentation Science amp Dynamic Measurement Ministry of Education School of Instrument and ElectronicsNorth University of China Taiyuan 030051 China2School of Electronics Engineering and Computer Science Peking University Beijing 100871 China

Correspondence should be addressed to Jun Liu liujnuceducn

Received 19 November 2015 Accepted 12 January 2016

Academic Editor Fadi Dornaika

Copyright copy 2016 Chong Shen et al This is an open access article distributed under the Creative Commons Attribution Licensewhich permits unrestricted use distribution and reproduction in any medium provided the original work is properly cited

The drift of inertial navigation system (INS) will lead to large navigation error when a low-cost INS is used in microaerialvehicles (MAV) To overcome the above problem an INSoptical flowmagnetometer integrated navigation scheme is proposedfor GPS-denied environment in this paper The scheme which is based on extended Kalman filter combines INS and optical flowinformation to estimate the velocity and position of MAVThe gyro accelerator and magnetometer information are fused togetherto estimate the MAV attitude when the MAV is at static state or uniformly moving state and the gyro only is used to estimatethe MAV attitude when the MAV is accelerating or decelerating The MAV flight data is used to verify the proposed integratednavigation scheme and the verification results show that the proposed scheme can effectively reduce the errors of navigationparameters and improve navigation precision

1 Introduction

Recently the autonomous operating unmanned aerial vehicle(MAV) is becoming popular in military and civilian applica-tions [1ndash3] such as aerial surveillance cargo delivery searchand rescue and remote sensing and mapping A reliablenavigation system is very important for safely operating theMAV Currently global positioning system (GPS) [4] andinertial navigation system (INS) [5] are the most widely usednavigation techniques for the MAV GPS is based on satellitewhich can provide relatively consistent accuracy navigationhowever the satellite signals would be lost in city canyon orindoor environments [6 7] INS is a self-contained devicewhich can offer a complete set of navigation parameters withhigh frequency including attitude velocity and positionhowever the rapid growth of systematic errors with time isa main drawback of stand-alone INS [8 9]

Due to the drawbacks of GPS and INS lots of novelnavigation devices have been studied For example opticalflow sensor has been proved which can provide reliable

velocity and position information when applied for robotlocalization or navigation [10 11] Optical flow techniquesare motivated by bird and insect flights which are used tosolve the navigation problem Optical flow can be thoughtto be the 2D projection of the 3D perceived motion of theobjects which has been widely utilized formotion estimation[12 13] When facing the ground the optical flow sensors canbe used for accurate velocity calculation and the position canbe obtained by integration For example computer mousesensor is a successful application of optical flow theoryBenefiting from previous study of optical flow algorithm andhardware design robotics researchers have employed opticalflow sensors for the MAV navigation Ding et al [14] addedoptical flow into the GPSINS integration for UAVnavigationwhere optical flow rate is used as backup velocity aiding incase ofGPS signal outages and the verification results showedthat by using optical flow the height estimation was stillwithin a meter range within 20 seconds after GPS droppedoutMercado et al [15] proposed a GPSINSoptical flow data

Hindawi Publishing CorporationJournal of SensorsVolume 2016 Article ID 6105803 10 pageshttpdxdoiorg10115520166105803

2 Journal of Sensors

Search area

Target block

Preceding frame Current frame

i

j

Optical flow vector

Target block in current frame

xp(i j)

d

xc(i + Δi j + Δj)

Δi

V Δj

Figure 1 Block matching algorithm based on sum of absolute differences

fusion method for position and velocity estimation experi-ment results showed that a good velocity measurement couldbe obtained by optical flow under rough GPS conditionsRhudy et al [16] aided wide-field optical flow to INS for UAVwhere the wide-field optical flow was used to regulate INSdrift for the purpose of GPS-denied navigation Gageik et al[17] applied an optical flow sensor for 2D positioning of UAVand position hold result showed that the standard deviationof the position error was just 10 cm and the position error wasjust about 30 cm after landing It can be clearly seen that theoptical flow is an effective solution for velocity and positionmeasurement However traditional optical flow is detectedby video camera which is not suitable for MAV because ofthe weight size and power In 2013 an optical flow sensorbased on a machine vision CMOS image sensor for MAVwas proposed and named as PX4FLOW [18] and the indoorverification results showed that the error was just 05m withthe overall trajectory being 2844m Compared to traditionaloptical flow sensors PX4FLOW is of low power low latencysmall size and low cost therefore it is very suitable for MAVapplications

In this paper an optical flow sensorINSmagnetometerintegrated navigation system is proposed for MAV whereoptical flow sensor is used to measure the velocity andposition of the MAV and the magnetometer is used tomeasure the attitude of the MAV then these navigationparameters are applied to calibrate the drift of INS by usingan extend Kalman filter This paper is organized as followsSection 2 is the theory of velocity and position estimationby optical flow The data fusion model is given in Section 3Section 4 is the experiment and verification Section 5 is theconclusion

2 Estimation of Velocity and Position byOptical Flow Sensor

21 The Calculation of Optical Flow Optical flow is the pro-jection of 3D relative motion onto a 2D image plane In order

to calculate the optical flow the researchers have proposedmany solutions such as Lucas-Kanade algorithm Horn-Schunck algorithm image interpolation algorithm blockmatching algorithm and feature matching algorithm In ourstudy considering the complexity of the software calculationand hardware platform the blockmatching algorithm (BMA)based on minimum mean absolute error (MMAE) and thesum of absolute differences (SAD) are selected to calculatethe optical flow which is shown in Figure 1

As shown in Figure 1 119909119901(119894 119895) is set as the gray value of119899 times 119899 target block selected from the previous frame and119909119888(119894 + Δ119894 119895 + Δ119895) is the gray value of the target block which isto be matched in the searching area of current frame where1 le 119894 119895 le 119899 minus119889 le Δ119894 Δ119895 le 119889 The principle of BMA based onMMAE and the SAD is to search Δ119894 and Δ119895 which satisfy (1)then the optical flow vector V = 119903(Δ119894 Δ119895)

119879 can be obtainedwhere the unit of V is pixelsec 119903 is the sample frequencyof the camera and the unit is framesec 119880 is the minimummean absolute error

SAD (Δ119894 Δ119895) =119899

sum

119895=1

119899

sum

119895=1

10038161003816100381610038161003816119909119888 (119894 + Δ119894 119895 + Δ119895) minus 119909119901 (119894 119895)

10038161003816100381610038161003816 (1)

119880 = min(Δ119894Δ119895)

SAD (Δ119894 Δ119895) (2)

V = 119903 (Δ119894 Δ119895)119879100381610038161003816100381610038161003816119880 (3)

In the initial state a target is selected at the original pointof the imaging plane the target block will move when theMAV moves In the searching area of the current frame theoptical flow vector of the target block can be obtained bycalculating theminimumSADbetween the current block andthe previous block

In the experiment the images which are perpendicularto the camera are collected In the whole collecting process adata block which is 8 times 8 pixel is used as the block matchingobject and the searching area contains plusmn4 pixels So there

Journal of Sensors 3

Pc (Xc Yc Zc)

p (x y f)

f

XcYc

O

Zc

Figure 2 The model of pin-hole image plane

are 64 pixel points and 81 candidate vector directions in eachframe image After obtaining each frame image the meanabsolute errors of each candidate vector are calculated andthe minimum value is selected as the optical flow vector

22 The Model of Optical Flow The motion model of opticalflow is projecting the three-dimensional motion onto thetwo-dimensional image plane of the camera There are twocommon optical flow estimation models one is pin-holeimage plane approach which is derived from the principleof insect and vertebrate visual system the other is sphericalimaging surface approach which is derived from insectcompound eyes In our study the pin-hole image planeapproach is used to estimate the motion of MAV undergeographic coordinate system

The pin-hole image plane model is shown as in Figure 2Pc = [119883119888 119884119888 119885119888]

119879 is set as a point under camera coordinatesystem 119891 stands for the focal length so Pc can be expressedas p = [119909 119910 119891]119879

p = 119891Pc119885119888

(4)

119909 = 119891

119883119888

119885119888

(5)

119910 = 119891

119884119888

119885119888

(6)

Considering any point P on the ground point P has thefollowing relationship relative to the MAV under the cameracoordinate system

Vc = minusTc minus 120596 times Pc (7)

Equation (7) is expanded in three dimensions and (8) canbe obtained

119881119909119888

= minus119879119909119888

minus (120596119910119885119888 minus 120596119911119884119888)

119881119910119888

= minus119879119910119888

minus (120596119911119883119888 minus 120596119909119885119888)

119881119911119888

= minus119879119911119888

minus (120596119909119884119888 minus 120596119910119883119888)

(8)

where 120596 = [120596119909 120596119910 120596119911]119879 is angular velocity of MAV Tc =

[119879119909119888

119879119910119888

119879119911119888

]119879 is average velocity of MAV under camera

coordinate systemAfter derivation calculus to (4) the relationship between

the velocity ofPc under the camera coordinate system and thevelocity of p under the image plane can be obtained

flowΔtime

= k = 119891119885Vc minus 119881119911Pc

1198852119888

(9)

where k = [V119909 V119910 V119911]119879 and (10)ndash(12) can obtained by

expanding (9)

V119909 =119891

1198852119888

(119881119909119888

119885119888 minus 119881119911119885119909) (10)

V119910 =119891

1198852119888

(119881119910119888

119885119888 minus 119881119911119885119910) (11)

V119911 = 0 (12)

Substituting (8) into (10) and (11) (13) can be obtained

V119909 =119879119911119888

119909 minus 119879119909119888

119891

119885119888

minus 120596119910119891 + 120596119911119910 +

120596119909119909119910 minus 1205961199101199092

119891

V119910 =119879119911119888

119910 minus 119879119910119888

119891

119885119888

+ 120596119909119891 minus 120596119909119910 +

1205961199091199102minus 120596119910119909119910

119891

(13)

In (13) V119909 and V119910 are the optical flow components ondirections119909 and119910 which can be calculated by SADandBMA119885119888 can be obtained by ultrasonic which was integrated in theoptical flow sensor Angular velocity 120596119909 120596119910 and 120596119911 can beobtained by gyroscope 119909 and 119910 can be substituted by (2)and (3) Therefore the translational velocity Tc of the MAVunder the camera coordinate system can be estimated andtheMAV velocity under geographic coordinate system can becalculated through coordinate transformational matrix Cn

c After integration of velocity the data for position ofMAV canbe obtained at last

23 Attitude Estimation Based on AccelerometerMagnetome-ter By measuring the gravitational field the accelerometercan determine the roll and pitch of the MAV under thecondition of no own acceleration by measuring the geomag-netic field the magnetometer can determine the heading ofMAV based on the vehiclersquos attitude information provided byaccelerometer Then the whole attitude information withoutaccumulated error can be obtained by integrating accelerom-etermagnetometer

231 The Roll and Pitch Obtained by Accelerometer Thecomponent of gravity vector under the geographic coordi-nate system is [0 0 minus119892]

119879 When the vehicle is static (norelative acceleration to the navigation coordinate system)the measured value of accelerometer which is fixed in thecarrier coordinate system is a119887 = [119886119909

119887

119886119910119887

119886119911119887

]

119879 Theheading of the vehicle has no influence on the output of


accelerometers of directions 119909 and 119910 due to the gravitationalacceleration perpendicular to horizontal plane Therefore itcan be concluded that

[

[

[

[

119886119909119887

119886119910119887

119886119911119887

]

]

]

]

=[

[

[

cos 120574 sin 120574 sin 120579 minus sin 120574 cos 1205790 cos 120579 sin 120579

sin 120574 minus cos 120574 sin 120579 cos 120574 cos 120579

]

]

]

[

[

[

0

0

minus119892

]

]

]

(14)

The roll and pitch can be calculated

120579 = arcsin(minus119886119910119887

119892

)

120574 = arctan(minus119886119909119887

119886119911119887

)

(15)

Thismethod uses the projection information of the earthrsquosgravitational acceleration in carrier coordinate system toreflect the attitude information of the vehicle so the above

equations are correct only under the condition that there isno acceleration of the vehicle Actually the vehicles are notalways of static or uniform motion and the measured valuesof accelerometer are not equal to the component of gravita-tional acceleration under MAV coordinate system anymoreonce the carrier is under accelerated motion Therefore thismethod only can be used for measurement in static state andit is necessary to find another attitude measuring method fordynamic conditions

232 The Heading Obtained by Magnetometer The com-ponent of earthrsquos magnetic field intensity under geographiccoordinate system is Hn = [119867119909

119899

119867119910119899

119867119911119899

]

119879 The magne-tometers are fixed on the carrier and the coordinate of themagnetometers is the same as the carrier coordinate system119865119887 and the component of magnetic field intensity under thecarrier coordinate system is Hb = [119867119909

119887

119867119910119887

119867119911119887

]

119879 Theprojection of magnetic field intensity to the geographic andcamera coordinate systems can be presented as

[

[

[

[

119867119909119899

119867119910119899

119867119911119899

]

]

]

]

=[

[

[

cos 120574 cos120593 minus sin 120574 sin 120579 sin120593 minus cos 120579 sin120593 sin 120574 cos120593 + cos 120574 sin 120579 sin120593cos 120574 sin120593 + sin 120574 sin 120579 cos120593 cos 120579 cos120593 sin 120574 sin120593 minus cos 120574 sin 120579 cos120593

minus sin 120574 cos 120579 sin 120579 cos 120574 cos 120579

]

]

]

[

[

[

[

119867119909119887

119867119910119887

119867119911119887

]

]

]

]

(16)

where [119867119909119899

119867119910119899

119867119911119899

]

119879 can be referred to by [19] InTaiyuan area (northern latitude 378∘ east longitude1125∘) [119867119909

119899

119867119910119899

119867119911119899

]

119879 is shown as (17) and the data of[119867119909119887

119867119910119887

119867119911119887

]

119879 can be obtained from the magnetometer

119867119909119899

= minus0216 times 10minus5119879

119867119910119899

= 0309 times 10minus4119879

119867119911119899


(17)

Supposing that themagnetic field intensity keeps on beingconstant during the flight ofMAV the heading ofMAVundergeographic coordinate system can be calculated by (16) and(17) based on the roll and pitch provided by accelerometer

3 Data Fusion Based on EKF

31 Optical Flow SensorAccelerometer Integrated System forVelocity and Position Measurements In our study the EKFfilter is employed to fuse the accelerometer and optical flowsensorThe velocity and position calculated by accelerometerunder navigation coordinates are selected as the state valueand the velocity and position calculated by optical flow sensorare selected as the observer value The estimation process isshown in Figure 3

Considering the nonlinear system state equation andobserved equation

X119896+1 = 119891 [X119896 119896] + B119896U119896 + Γ119896W119896

Z119896+1 = ℎ [X119896+1 119896 + 1] + V119896+1(18)

where X119896 = [119883119899 119884119899 119885119899 119881119909119899

119881119910119899

119881119911119899

]

119879 is state vectorwhich includes the velocity information and position infor-mation of MAV Z119896 = [119909 119910 119885119888]

119879 is observer vector whichincludes the optical flow data of directions 119909 and 119910 fromoptical flow sensor and the data 119885119888 obtained from ultrasonicsensors U119896 = [0 0 0 119886119909

119899

119886119910119899

119886119911119899

]

119879 is the control vector ofsystem that can be obtained by the accelerometer data aftercoordinate matrix transformation B119896 is matrix of controlallocation Γ119896 is matrix of noise allocation W119896 is matrix ofprocess noise V119896 is observer noise 119891 represents the systemstate function ℎ represents the observation function

Substituting the state and observed equation into EKF thetime renewal equation is

X119896+1119896 = 119891 [X119896 119896] + B119896U119896

P119896+1119896 = Φ119896+1119896P119896Φ119879

119896+1119896+ Γ119896Q119896Γ

119879

119896

(19)


Acc

Opticalflow

Sonar

Gyro

EKF

Position

Velocity

Zc

(Xn Yn Zn)

(Vx Vy Vz)

(ax ay az)

(120596x 120596y 120596z)

(x y)

Figure 3 Optical flow sensoraccelerometer integrated system

Acc

Magneto-meter

Gyro

ExtendedKalman

filter

Altitude

Gauss-Newtonmethod

4th-order Runge-Kutta

q(m)

q

q(i)

H

120596

a

(120601 120579 120574)

Figure 4 Gyroscopeaccelerometermagnetometer integrated sys-tem

Measurement renewal equation is

K119896+1 = P119896+1119896H119896+1 (H119896+1P119896+1119896H119879

119896+1+ R119896+1)

minus1

X119896+1 = X119896+1119896 + K119896+1 Z119896+1 minus ℎ [X119896+1119896 119896 + 1]

P119896+1 = (I minus K119896+1H119896+1)P119896+1119896 (I minus K119896+1H119896+1)minus1

+ K119896+1R119896+1K119879

119896+1

(20)

where Φ119896+1119896 = (120597119891[X119896 119896]120597X119879119896 )|X119896=X119896

and H119896+1119896 =

(120597ℎ[X119896+1 119896 + 1]120597X119879119896+1)|X119896+1=X119896+1119896

From the process of EKF mentioned above the data for

position and velocity of MAV can be obtained in geographiccoordinate system

32 GyroscopeAccelerometerMagnetometer Integrated Sys-tem Based on EKF The attitude of MAV can be obtainedthrough the integration of the angular rate from gyroscopeoutput signal however the performance ofMEMS gyroscopewould be influenced by drift The integrated accelerome-termagnetometer system can provide attitude informationwithout drift therefore it is necessary to fuse the data ofmultisensors by using EKF The filtering process is shownas Figure 4 the system state vector can be expressed asX119896 = [1199020 1199021 1199022 1199023 120596119909 120596119910 120596119911]

119879 where [1199020 1199021 1199022 1199023]119879 is

quaternion of the system state it can be calculated by fourth-order Runge-Kutta method [120596119909 120596119910 120596119911]

119879 is the output ofgyroscope

The system observation vector can be expressed as Z119896 =[1199020 1199021 1199022 1199023]

119879 which can be calculated by Gauss-Newtonmethod based on the data of accelerometer and magnetome-ter

33 Optical Flow SensorINSMagnetometer Integrated Navi-gation Scheme The effective estimation of MAV velocity andposition can be obtained by using optical flow sensorINSintegrated navigation system based on EKF no matter understationary status or motion status According to the char-acteristics of attitude measurements by using gyroscopeand accelerometermagnetometer accelerometer and mag-netometer are integrated to calibrate the gyroscope duringstationary or uniform motion state when the motion stateis detected as accelerating or decelerating the standalonegyroscopes are used to obtain the attitude by strap-downcalculatingThemotion state is detected by optic flow sensorThe overall navigation scheme is shown in Figure 5

4 Experiments and Verification

In order to verify the proposed optical flow sensorINSmagnetometer integrated navigation system a MAV flightexperiment was carried out The experiment location was inthe stadium of North University of China Taiyuan As shownin Figure 6 we can see that during the flight procedure theMAV kept flying to the north The flying distance was 50meters and the flight height is 15 meters

41 Experiments Platform As shown in Figures 7 and 8 theautonomous MAV is used as flight platform (Figure 7(a))STM32F103Z is selected as the flight control proces-sor (Figure 7(b)) the optical flow sensor is produced by3D Robtics Corporation which is named as PX4FLOW(Figure 7(c)) the IMU is MPU6050 (Figure 7(d)) the mag-netometer is HMC3883L which is produced by HoneywellLimited (Figure 7(d)) DJ1 2312 which is produced by DJI isselected as the MAVmotor (Figure 7(e)) and HC-12 wirelessserial interface module (Figure 7(f)) is used to transmit thesensorrsquos data fromMAV to PC

42 Verification Results Figures 9ndash11 are the velocity estima-tion results position estimation results and attitude estima-tion results respectively Table 1 is the end position compari-son results between INS only and OFSINSmagnetometer Itcan be concluded that (1) in the process of velocity estimationand position estimation the OFSINSmagnetometer basedon EKF can decrease the drift of INS effectively For examplefrom Figure 9 we can see that the position drift is about 8meters after 50 meters trajectory without the correction ofOFS and magnetometer and the position drift is decreasedto less than 1 meter after using the integrated algorithmand (2) in the process of attitude estimation the roll anglepitch angle and the yaw angle can be effectively tracked byOFSINSmagnetometer based on EKF during static and uni-form velocity status And after nonuniform velocity motionthe attitude can be corrected immediately


Table 1 Comparison results between INS only and OFSINSmagnetometer at the end position

Ground truth position at the end INS only OFSINSmagnetometerDirections Ground truth value Measurements Error Estimation Error119883 0 minus187 mdash 025 mdash119884 50 578 156 509 18119885 0 078 mdash 014 mdash

Start

Read the output ofoptical flow sensor

Stationary state oruniform motion

Attitude determination by

Output the attitude andupdate the transfer matrix

Velocity and position determinationby optical flow sensorINS

Attitude determinationby stand-alone gyro

gyroaccmagnetometer

Figure 5 The flow chart of optical flow sensorINSmagnetometer integrated navigation system

N

E

Start

End

Figure 6 The experiment trajectory


(b)

(c)

(d)(e)(f)

(a)

Figure 7 The hardware experiment platform

HC12MPU6050 PX4FLOW

Figure 8 The INSOFSmagnetometer integrated navigation system

Figure 12 is the trajectory comparison results betweenground truth INS only and EKF It can be concluded that theproposed optical flow sensorINSmagnetometer integratednavigation system can provide a higher accurate navigationresult compared to INS only navigation system

5 Conclusion

In this paper an optical flow sensorINSmagnetometerintegrated navigation system is proposed for MAV The

proposed method which is based on EKF combines opticalflow sensor INS and magnetometer information to estimatethe attitude velocity and position of MAV Specifically thegyro accelerator and magnetometer information are fusedtogether to estimate theMAV attitude when theMAV is staticor uniformlymoving and the gyro only is used to estimate theMAV attitude when the MAV is accelerating or deceleratingExperiment results show that the proposed method whichcan significantly reduce the errors for navigation positionvelocity and attitude compared with the INS only navigation


Time (s) Time (s) Time (s)0 10 15 20 25

Velo

city

(ms

)

Velo

city

(ms

)

Velo

city

(ms

)

minus02

0

02

04

06

08

1

INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25minus05

0

05

1

15

2

25

3

35

4

10 15 20 25minus08

minus06

minus04

minus02

0

02

04

06

08

1Use EKF to estimate the VzUse EKF to estimate the VyUse EKF to estimate the Vx

5 0 5 0 5

Figure 9 The velocity estimation results

10 15 20 25

Dist

ance

(m)

minus2

minus15

minus1

minus05

0

05

1Use EKF to estimate the X

INS onlyEKF

INS onlyEKF

INS onlyEKF

Time (s)Time (s) Time (s)0 10 15 20 25

minus10

0

10

20

30

40

50

60Use EKF to estimate the Y

10 15 20 25

Dist

ance

(m)

Dist

ance

(m)

0

05

1

15

2

25 Use EKF to estimate the Z

50 5 0 5

Figure 10 The position estimation results

system can effectively improve the navigation performanceof MAV with a significant value of engineering application

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper

Acknowledgments

Thisworkwas supported in part by the ChinaNational Fundsfor Distinguished Young Scientists (51225504) the National973 Program (2012CB723404) the National Natural ScienceFoundation of China (91123016 61171056) the ResearchProject Supported by Shanxi Scholarship Council of China


Time (s)0 10 15 20 25

Roll

angl

e (de

g)

minus9

minus8

minus7

minus6

minus5

minus4

minus3

minus2

minus1

0

1Use EKF to estimate the roll angle

Time (s) Time (s)5 10 15 20 25

Pitc

h an

gle (

deg

)

00 50

1

2

3

4

5

6Use EKF to estimate the pitch angle

INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25

Yaw

angl

e (de

g)

minus2

0

2

4

6

8

10

12Use EKF to estimate the yaw angle

5

Figure 11 The attitude estimation results

050

Distance X (m)minus05

minus1minus15

Trajectory

minus2minus200

Distance Y (m)

2040

15

1

0

2

05

25

60

Dist

ance

Z (m

)

EKFGround truthINS only

Figure 12 Trajectory comparison results between ground truth INSonly and EKF

(2015-082) and the College Funding of North University ofChina (110246)

References

[1] C Friedman I Chopra and O Rand ldquoIndooroutdoor scan-matching based mapping technique with a helicopter MAV inGPS-denied environmentrdquo International Journal of Micro AirVehicles vol 7 no 1 pp 55ndash70 2015

[2] Y Hu H L Zhang and C Tan ldquoThe effect of the aerofoilthickness on the performance of theMAV scale cycloidal rotorrdquoAeronautical Journal vol 119 pp 343ndash364 2015

[3] M Samani M Tafreshi I Shafieenejad and A A NikkhahldquoMinimum-time open-loop and closed-loop optimal guidancewith GA-PSO and neural fuzzy for Samarai MAV flightrdquo IEEEAerospace and Electronic System Magazine vol 30 pp 28ndash372015

[4] G Flores S T Zhou R Lozano and P Castillo ldquoA visionand GPS-based real-time trajectory planning for a MAV inunknown and low-sunlight environmentsrdquo Journal of Intelligentamp Robotic Systems vol 74 no 1-2 pp 59ndash67 2014

[5] A Ali and N El-Sheimy ldquoLow-cost MEMS-based pedestriannavigation technique for GPS-denied areasrdquo Journal of Sensorsvol 2013 Article ID 197090 10 pages 2013

[6] X Chen C Shen W-B Zhang M Tomizuka Y Xu andK Chiu ldquoNovel hybrid of strong tracking Kalman filter andwavelet neural network for GPSINS during GPS outagesrdquoMeasurement vol 46 no 10 pp 3847ndash3854 2013

[7] T Zhang and X S Xu ldquoA new method of seamless landnavigation for GPSINS integrated systemrdquo Measurement vol45 no 4 pp 691ndash701 2012

[8] R Song X Y Chen C Shen and H Zhang ldquoModelingFOG drift using back-propagation neural network optimizedby artificial fish swarm algorithmrdquo Journal of Sensors vol 2014Article ID 273043 6 pages 2014

[9] H Q Huang X Y Chen Z K Zhou Y Xu and C P Lv ldquoStudyof the algorithm of backtracking decoupling and adaptiveextended kalman filter based on the quaternion expanded to thestate variable for underwater glider navigationrdquo Sensors vol 14no 12 pp 23041ndash23066 2014

[10] D H Yi T H Lee and D I Cho ldquoAfocal optical flow sensor forreducing vertical height sensitivity in indoor robot localizationand navigationrdquo Sensors vol 15 no 5 pp 11208ndash11221 2015


[11] H Chao Y Gu and M Napolitano ldquoA survey of optical flowtechniques for UAV navigation applicationsrdquo in Proceedingsof the International Conference on Unmanned Aircraft Systems(ICUAS rsquo13) pp 710ndash716 IEEE Atlanta Ga USA May 2013

[12] F Martınez A Manzanera and E Romero ldquoAutomatic analysisand characterization of the hummingbird wings motion usingdense optical flow featuresrdquo Bioinspiration amp Biomimetics vol10 no 1 Article ID 016006 2015

[13] M P Rodriguez and A Nygren ldquoMotion estimation in cardiacfluorescence imaging with scale-space landmarks and opticalflow a comparative studyrdquo IEEE Transactions on BiomedicalEngineering vol 62 no 2 pp 774ndash782 2015

[14] W D Ding J LWang S L Han et al ldquoAdding optical flow intothe GPSINS integration for UAV navigationrdquo in Proceedings ofthe International Global Navigation Satellite Systems Society pp1ndash13 Surfers Paradise Australia 2009

[15] D AMercado G Flores P Castillo J Escareno and R LozanoldquoGPSINSOptic flow data fusion for position and velocityestimationrdquo in Proceedings of the International Conference onUnmanned Aircraft Systems (ICUAS rsquo13) pp 486ndash491 AtlantaGa USA May 2013

[16] M B Rhudy H Y Chao and Y Gu ldquoWide-field opticalflow aided inertial navigation for unmanned aerial vehiclesrdquoin Proceedings of the IEEERSJ International Conference onIntelligent Robots and Systems (IROS rsquo14) pp 674ndash679 IEEEChicago Ill USA September 2014

[17] K Gageik M Strohmeier and S Montenegro ldquoAn autonomousUAV with an optical flow sensor for positioning and naviga-tionrdquo International Journal of Advanced Robotic Systems vol 10pp 1ndash9 2013

[18] D Honegger L Meier P Tanskanen and M Pollefeys ldquoAnopen source and open hardware embedded metric opticalflow CMOS camera for indoor and outdoor applicationsrdquo inProceedings of the IEEE International Conference on Roboticsand Automation (ICRA rsquo13) pp 1736ndash1741 Karlsruhe GermanyMay 2013

[19] H-D Zhao H-S Cao J-Z Zhu and H-S Zhang ldquoMethodof attitude measurement with magnetometer and gyroscoperdquoJournal of North University of China vol 31 no 6 pp 631ndash6352010 (Chinese)

International Journal of

AerospaceEngineeringHindawi Publishing Corporationhttpwwwhindawicom Volume 2014

RoboticsJournal of

Hindawi Publishing Corporationhttpwwwhindawicom Volume 2014


Active and Passive Electronic Components

Control Scienceand Engineering

Journal of



RotatingMachinery


Hindawi Publishing Corporation httpwwwhindawicom

Journal ofEngineeringVolume 2014

Submit your manuscripts athttpwwwhindawicom

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics


Volume 2014

The Scientific World JournalHindawi Publishing Corporation httpwwwhindawicom Volume 2014

SensorsJournal of


Modelling amp Simulation in EngineeringHindawi Publishing Corporation httpwwwhindawicom Volume 2014


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation



DistributedSensor Networks



Search area

Target block

Preceding frame Current frame

i

j

Optical flow vector

Target block in current frame

xp(i j)

d

xc(i + Δi j + Δj)

Δi

V Δj

Figure 1 Block matching algorithm based on sum of absolute differences

fusion method for position and velocity estimation experi-ment results showed that a good velocity measurement couldbe obtained by optical flow under rough GPS conditionsRhudy et al [16] aided wide-field optical flow to INS for UAVwhere the wide-field optical flow was used to regulate INSdrift for the purpose of GPS-denied navigation Gageik et al[17] applied an optical flow sensor for 2D positioning of UAVand position hold result showed that the standard deviationof the position error was just 10 cm and the position error wasjust about 30 cm after landing It can be clearly seen that theoptical flow is an effective solution for velocity and positionmeasurement However traditional optical flow is detectedby video camera which is not suitable for MAV because ofthe weight size and power In 2013 an optical flow sensorbased on a machine vision CMOS image sensor for MAVwas proposed and named as PX4FLOW [18] and the indoorverification results showed that the error was just 05m withthe overall trajectory being 2844m Compared to traditionaloptical flow sensors PX4FLOW is of low power low latencysmall size and low cost therefore it is very suitable for MAVapplications

In this paper an optical flow sensorINSmagnetometerintegrated navigation system is proposed for MAV whereoptical flow sensor is used to measure the velocity andposition of the MAV and the magnetometer is used tomeasure the attitude of the MAV then these navigationparameters are applied to calibrate the drift of INS by usingan extend Kalman filter This paper is organized as followsSection 2 is the theory of velocity and position estimationby optical flow The data fusion model is given in Section 3Section 4 is the experiment and verification Section 5 is theconclusion

2 Estimation of Velocity and Position byOptical Flow Sensor

21 The Calculation of Optical Flow Optical flow is the pro-jection of 3D relative motion onto a 2D image plane In order

to calculate the optical flow the researchers have proposedmany solutions such as Lucas-Kanade algorithm Horn-Schunck algorithm image interpolation algorithm blockmatching algorithm and feature matching algorithm In ourstudy considering the complexity of the software calculationand hardware platform the blockmatching algorithm (BMA)based on minimum mean absolute error (MMAE) and thesum of absolute differences (SAD) are selected to calculatethe optical flow which is shown in Figure 1

As shown in Figure 1 119909119901(119894 119895) is set as the gray value of119899 times 119899 target block selected from the previous frame and119909119888(119894 + Δ119894 119895 + Δ119895) is the gray value of the target block which isto be matched in the searching area of current frame where1 le 119894 119895 le 119899 minus119889 le Δ119894 Δ119895 le 119889 The principle of BMA based onMMAE and the SAD is to search Δ119894 and Δ119895 which satisfy (1)then the optical flow vector V = 119903(Δ119894 Δ119895)

119879 can be obtainedwhere the unit of V is pixelsec 119903 is the sample frequencyof the camera and the unit is framesec 119880 is the minimummean absolute error

SAD (Δ119894 Δ119895) =119899

sum

119895=1

119899

sum

119895=1

10038161003816100381610038161003816119909119888 (119894 + Δ119894 119895 + Δ119895) minus 119909119901 (119894 119895)

10038161003816100381610038161003816 (1)

119880 = min(Δ119894Δ119895)

SAD (Δ119894 Δ119895) (2)

V = 119903 (Δ119894 Δ119895)119879100381610038161003816100381610038161003816119880 (3)

In the initial state a target is selected at the original pointof the imaging plane the target block will move when theMAV moves In the searching area of the current frame theoptical flow vector of the target block can be obtained bycalculating theminimumSADbetween the current block andthe previous block

In the experiment the images which are perpendicularto the camera are collected In the whole collecting process adata block which is 8 times 8 pixel is used as the block matchingobject and the searching area contains plusmn4 pixels So there


Pc (Xc Yc Zc)

p (x y f)

f

XcYc

O

Zc






p = 119891Pc119885119888

(4)

119909 = 119891

119883119888

119885119888

(5)

119910 = 119891

119884119888

119885119888

(6)




119881119909119888

= minus119879119909119888

minus (120596119910119885119888 minus 120596119911119884119888)

119881119910119888

= minus119879119910119888

minus (120596119911119883119888 minus 120596119909119885119888)

119881119911119888

= minus119879119911119888

minus (120596119909119884119888 minus 120596119910119883119888)

(8)


[119879119909119888

119879119910119888

119879119911119888




flowΔtime

= k = 119891119885Vc minus 119881119911Pc

1198852119888

(9)


expanding (9)

V119909 =119891

1198852119888

(119881119909119888

119885119888 minus 119881119911119885119909) (10)

V119910 =119891

1198852119888

(119881119910119888

119885119888 minus 119881119911119885119910) (11)

V119911 = 0 (12)


V119909 =119879119911119888

119909 minus 119879119909119888

119891

119885119888

minus 120596119910119891 + 120596119911119910 +

120596119909119909119910 minus 1205961199101199092

119891

V119910 =119879119911119888

119910 minus 119879119910119888

119891

119885119888

+ 120596119909119891 minus 120596119909119910 +

1205961199091199102minus 120596119910119909119910

119891

(13)






119887

119886119910119887

119886119911119887

]




[

[

[

[

119886119909119887

119886119910119887

119886119911119887

]

]

]

]

=[

[

[



]

]

]

[

[

[

0

0

minus119892

]

]

]

(14)


120579 = arcsin(minus119886119910119887

119892

)

120574 = arctan(minus119886119909119887

119886119911119887

)

(15)




119899

119867119910119899

119867119911119899

]


119887

119867119910119887

119867119911119887

]


[

[

[

[

119867119909119899

119867119910119899

119867119911119899

]

]

]

]

=[

[

[



]

]

]

[

[

[

[

119867119909119887

119867119910119887

119867119911119887

]

]

]

]

(16)

where [119867119909119899

119867119910119899

119867119911119899

]


119899

119867119910119899

119867119911119899

]


119867119910119887

119867119911119887

]


119867119909119899


119867119910119899

= 0309 times 10minus4119879

119867119911119899


(17)





X119896+1 = 119891 [X119896 119896] + B119896U119896 + Γ119896W119896

Z119896+1 = ℎ [X119896+1 119896 + 1] + V119896+1(18)

where X119896 = [119883119899 119884119899 119885119899 119881119909119899

119881119910119899

119881119911119899

]



119899

119886119910119899

119886119911119899

]



X119896+1119896 = 119891 [X119896 119896] + B119896U119896

P119896+1119896 = Φ119896+1119896P119896Φ119879

119896+1119896+ Γ119896Q119896Γ

119879

119896

(19)


Acc

Opticalflow

Sonar

Gyro

EKF

Position

Velocity

Zc

(Xn Yn Zn)

(Vx Vy Vz)

(ax ay az)

(120596x 120596y 120596z)

(x y)


Acc

Magneto-meter

Gyro

ExtendedKalman

filter

Altitude

Gauss-Newtonmethod


q(m)

q

q(i)

H

120596

a

(120601 120579 120574)



K119896+1 = P119896+1119896H119896+1 (H119896+1P119896+1119896H119879

119896+1+ R119896+1)

minus1

X119896+1 = X119896+1119896 + K119896+1 Z119896+1 minus ℎ [X119896+1119896 119896 + 1]


+ K119896+1R119896+1K119879

119896+1

(20)

where Φ119896+1119896 = (120597119891[X119896 119896]120597X119879119896 )|X119896=X119896

and H119896+1119896 =

(120597ℎ[X119896+1 119896 + 1]120597X119879119896+1)|X119896+1=X119896+1119896




119879 where [1199020 1199021 1199022 1199023]119879 is













Start







gyroaccmagnetometer


N

E

Start

End



(b)

(c)

(d)(e)(f)

(a)


HC12MPU6050 PX4FLOW



5 Conclusion





Velo

city

(ms

)

Velo

city

(ms

)

Velo

city

(ms

)

minus02

0

02

04

06

08

1

INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25minus05

0

05

1

15

2

25

3

35

4

10 15 20 25minus08

minus06

minus04

minus02

0

02

04

06

08


5 0 5 0 5


10 15 20 25

Dist

ance

(m)

minus2

minus15

minus1

minus05

0

05


INS onlyEKF

INS onlyEKF

INS onlyEKF


minus10

0

10

20

30

40

50


10 15 20 25

Dist

ance

(m)

Dist

ance

(m)

0

05

1

15

2


50 5 0 5





Acknowledgments



Time (s)0 10 15 20 25

Roll

angl

e (de

g)

minus9

minus8

minus7

minus6

minus5

minus4

minus3

minus2

minus1

0


Time (s) Time (s)5 10 15 20 25

Pitc

h an

gle (

deg

)

00 50

1

2

3

4

5


INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25

Yaw

angl

e (de

g)

minus2

0

2

4

6

8

10


5


050


minus1minus15

Trajectory

minus2minus200

Distance Y (m)

2040

15

1

0

2

05

25

60

Dist

ance

Z (m

)




References























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










Pc (Xc Yc Zc)

p (x y f)

f

XcYc

O

Zc






p = 119891Pc119885119888

(4)

119909 = 119891

119883119888

119885119888

(5)

119910 = 119891

119884119888

119885119888

(6)




119881119909119888

= minus119879119909119888

minus (120596119910119885119888 minus 120596119911119884119888)

119881119910119888

= minus119879119910119888

minus (120596119911119883119888 minus 120596119909119885119888)

119881119911119888

= minus119879119911119888

minus (120596119909119884119888 minus 120596119910119883119888)

(8)


[119879119909119888

119879119910119888

119879119911119888




flowΔtime

= k = 119891119885Vc minus 119881119911Pc

1198852119888

(9)


expanding (9)

V119909 =119891

1198852119888

(119881119909119888

119885119888 minus 119881119911119885119909) (10)

V119910 =119891

1198852119888

(119881119910119888

119885119888 minus 119881119911119885119910) (11)

V119911 = 0 (12)


V119909 =119879119911119888

119909 minus 119879119909119888

119891

119885119888

minus 120596119910119891 + 120596119911119910 +

120596119909119909119910 minus 1205961199101199092

119891

V119910 =119879119911119888

119910 minus 119879119910119888

119891

119885119888

+ 120596119909119891 minus 120596119909119910 +

1205961199091199102minus 120596119910119909119910

119891

(13)






119887

119886119910119887

119886119911119887

]




[

[

[

[

119886119909119887

119886119910119887

119886119911119887

]

]

]

]

=[

[

[



]

]

]

[

[

[

0

0

minus119892

]

]

]

(14)


120579 = arcsin(minus119886119910119887

119892

)

120574 = arctan(minus119886119909119887

119886119911119887

)

(15)




119899

119867119910119899

119867119911119899

]


119887

119867119910119887

119867119911119887

]


[

[

[

[

119867119909119899

119867119910119899

119867119911119899

]

]

]

]

=[

[

[



]

]

]

[

[

[

[

119867119909119887

119867119910119887

119867119911119887

]

]

]

]

(16)

where [119867119909119899

119867119910119899

119867119911119899

]


119899

119867119910119899

119867119911119899

]


119867119910119887

119867119911119887

]


119867119909119899


119867119910119899

= 0309 times 10minus4119879

119867119911119899


(17)





X119896+1 = 119891 [X119896 119896] + B119896U119896 + Γ119896W119896

Z119896+1 = ℎ [X119896+1 119896 + 1] + V119896+1(18)

where X119896 = [119883119899 119884119899 119885119899 119881119909119899

119881119910119899

119881119911119899

]



119899

119886119910119899

119886119911119899

]



X119896+1119896 = 119891 [X119896 119896] + B119896U119896

P119896+1119896 = Φ119896+1119896P119896Φ119879

119896+1119896+ Γ119896Q119896Γ

119879

119896

(19)


Acc

Opticalflow

Sonar

Gyro

EKF

Position

Velocity

Zc

(Xn Yn Zn)

(Vx Vy Vz)

(ax ay az)

(120596x 120596y 120596z)

(x y)


Acc

Magneto-meter

Gyro

ExtendedKalman

filter

Altitude

Gauss-Newtonmethod


q(m)

q

q(i)

H

120596

a

(120601 120579 120574)



K119896+1 = P119896+1119896H119896+1 (H119896+1P119896+1119896H119879

119896+1+ R119896+1)

minus1

X119896+1 = X119896+1119896 + K119896+1 Z119896+1 minus ℎ [X119896+1119896 119896 + 1]


+ K119896+1R119896+1K119879

119896+1

(20)

where Φ119896+1119896 = (120597119891[X119896 119896]120597X119879119896 )|X119896=X119896

and H119896+1119896 =

(120597ℎ[X119896+1 119896 + 1]120597X119879119896+1)|X119896+1=X119896+1119896




119879 where [1199020 1199021 1199022 1199023]119879 is













Start







gyroaccmagnetometer


N

E

Start

End



(b)

(c)

(d)(e)(f)

(a)


HC12MPU6050 PX4FLOW



5 Conclusion





Velo

city

(ms

)

Velo

city

(ms

)

Velo

city

(ms

)

minus02

0

02

04

06

08

1

INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25minus05

0

05

1

15

2

25

3

35

4

10 15 20 25minus08

minus06

minus04

minus02

0

02

04

06

08


5 0 5 0 5


10 15 20 25

Dist

ance

(m)

minus2

minus15

minus1

minus05

0

05


INS onlyEKF

INS onlyEKF

INS onlyEKF


minus10

0

10

20

30

40

50


10 15 20 25

Dist

ance

(m)

Dist

ance

(m)

0

05

1

15

2


50 5 0 5





Acknowledgments



Time (s)0 10 15 20 25

Roll

angl

e (de

g)

minus9

minus8

minus7

minus6

minus5

minus4

minus3

minus2

minus1

0


Time (s) Time (s)5 10 15 20 25

Pitc

h an

gle (

deg

)

00 50

1

2

3

4

5


INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25

Yaw

angl

e (de

g)

minus2

0

2

4

6

8

10


5


050


minus1minus15

Trajectory

minus2minus200

Distance Y (m)

2040

15

1

0

2

05

25

60

Dist

ance

Z (m

)




References























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











[

[

[

[

119886119909119887

119886119910119887

119886119911119887

]

]

]

]

=[

[

[



]

]

]

[

[

[

0

0

minus119892

]

]

]

(14)


120579 = arcsin(minus119886119910119887

119892

)

120574 = arctan(minus119886119909119887

119886119911119887

)

(15)




119899

119867119910119899

119867119911119899

]


119887

119867119910119887

119867119911119887

]


[

[

[

[

119867119909119899

119867119910119899

119867119911119899

]

]

]

]

=[

[

[



]

]

]

[

[

[

[

119867119909119887

119867119910119887

119867119911119887

]

]

]

]

(16)

where [119867119909119899

119867119910119899

119867119911119899

]


119899

119867119910119899

119867119911119899

]


119867119910119887

119867119911119887

]


119867119909119899


119867119910119899

= 0309 times 10minus4119879

119867119911119899


(17)





X119896+1 = 119891 [X119896 119896] + B119896U119896 + Γ119896W119896

Z119896+1 = ℎ [X119896+1 119896 + 1] + V119896+1(18)

where X119896 = [119883119899 119884119899 119885119899 119881119909119899

119881119910119899

119881119911119899

]



119899

119886119910119899

119886119911119899

]



X119896+1119896 = 119891 [X119896 119896] + B119896U119896

P119896+1119896 = Φ119896+1119896P119896Φ119879

119896+1119896+ Γ119896Q119896Γ

119879

119896

(19)


Acc

Opticalflow

Sonar

Gyro

EKF

Position

Velocity

Zc

(Xn Yn Zn)

(Vx Vy Vz)

(ax ay az)

(120596x 120596y 120596z)

(x y)


Acc

Magneto-meter

Gyro

ExtendedKalman

filter

Altitude

Gauss-Newtonmethod


q(m)

q

q(i)

H

120596

a

(120601 120579 120574)



K119896+1 = P119896+1119896H119896+1 (H119896+1P119896+1119896H119879

119896+1+ R119896+1)

minus1

X119896+1 = X119896+1119896 + K119896+1 Z119896+1 minus ℎ [X119896+1119896 119896 + 1]


+ K119896+1R119896+1K119879

119896+1

(20)

where Φ119896+1119896 = (120597119891[X119896 119896]120597X119879119896 )|X119896=X119896

and H119896+1119896 =

(120597ℎ[X119896+1 119896 + 1]120597X119879119896+1)|X119896+1=X119896+1119896




119879 where [1199020 1199021 1199022 1199023]119879 is













Start







gyroaccmagnetometer


N

E

Start

End



(b)

(c)

(d)(e)(f)

(a)


HC12MPU6050 PX4FLOW



5 Conclusion





Velo

city

(ms

)

Velo

city

(ms

)

Velo

city

(ms

)

minus02

0

02

04

06

08

1

INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25minus05

0

05

1

15

2

25

3

35

4

10 15 20 25minus08

minus06

minus04

minus02

0

02

04

06

08


5 0 5 0 5


10 15 20 25

Dist

ance

(m)

minus2

minus15

minus1

minus05

0

05


INS onlyEKF

INS onlyEKF

INS onlyEKF


minus10

0

10

20

30

40

50


10 15 20 25

Dist

ance

(m)

Dist

ance

(m)

0

05

1

15

2


50 5 0 5





Acknowledgments



Time (s)0 10 15 20 25

Roll

angl

e (de

g)

minus9

minus8

minus7

minus6

minus5

minus4

minus3

minus2

minus1

0


Time (s) Time (s)5 10 15 20 25

Pitc

h an

gle (

deg

)

00 50

1

2

3

4

5


INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25

Yaw

angl

e (de

g)

minus2

0

2

4

6

8

10


5


050


minus1minus15

Trajectory

minus2minus200

Distance Y (m)

2040

15

1

0

2

05

25

60

Dist

ance

Z (m

)




References























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










Acc

Opticalflow

Sonar

Gyro

EKF

Position

Velocity

Zc

(Xn Yn Zn)

(Vx Vy Vz)

(ax ay az)

(120596x 120596y 120596z)

(x y)


Acc

Magneto-meter

Gyro

ExtendedKalman

filter

Altitude

Gauss-Newtonmethod


q(m)

q

q(i)

H

120596

a

(120601 120579 120574)



K119896+1 = P119896+1119896H119896+1 (H119896+1P119896+1119896H119879

119896+1+ R119896+1)

minus1

X119896+1 = X119896+1119896 + K119896+1 Z119896+1 minus ℎ [X119896+1119896 119896 + 1]


+ K119896+1R119896+1K119879

119896+1

(20)

where Φ119896+1119896 = (120597119891[X119896 119896]120597X119879119896 )|X119896=X119896

and H119896+1119896 =

(120597ℎ[X119896+1 119896 + 1]120597X119879119896+1)|X119896+1=X119896+1119896




119879 where [1199020 1199021 1199022 1199023]119879 is













Start







gyroaccmagnetometer


N

E

Start

End



(b)

(c)

(d)(e)(f)

(a)


HC12MPU6050 PX4FLOW



5 Conclusion





Velo

city

(ms

)

Velo

city

(ms

)

Velo

city

(ms

)

minus02

0

02

04

06

08

1

INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25minus05

0

05

1

15

2

25

3

35

4

10 15 20 25minus08

minus06

minus04

minus02

0

02

04

06

08


5 0 5 0 5


10 15 20 25

Dist

ance

(m)

minus2

minus15

minus1

minus05

0

05


INS onlyEKF

INS onlyEKF

INS onlyEKF


minus10

0

10

20

30

40

50


10 15 20 25

Dist

ance

(m)

Dist

ance

(m)

0

05

1

15

2


50 5 0 5





Acknowledgments



Time (s)0 10 15 20 25

Roll

angl

e (de

g)

minus9

minus8

minus7

minus6

minus5

minus4

minus3

minus2

minus1

0


Time (s) Time (s)5 10 15 20 25

Pitc

h an

gle (

deg

)

00 50

1

2

3

4

5


INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25

Yaw

angl

e (de

g)

minus2

0

2

4

6

8

10


5


050


minus1minus15

Trajectory

minus2minus200

Distance Y (m)

2040

15

1

0

2

05

25

60

Dist

ance

Z (m

)




References























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation












Start







gyroaccmagnetometer


N

E

Start

End



(b)

(c)

(d)(e)(f)

(a)


HC12MPU6050 PX4FLOW



5 Conclusion





Velo

city

(ms

)

Velo

city

(ms

)

Velo

city

(ms

)

minus02

0

02

04

06

08

1

INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25minus05

0

05

1

15

2

25

3

35

4

10 15 20 25minus08

minus06

minus04

minus02

0

02

04

06

08


5 0 5 0 5


10 15 20 25

Dist

ance

(m)

minus2

minus15

minus1

minus05

0

05


INS onlyEKF

INS onlyEKF

INS onlyEKF


minus10

0

10

20

30

40

50


10 15 20 25

Dist

ance

(m)

Dist

ance

(m)

0

05

1

15

2


50 5 0 5





Acknowledgments



Time (s)0 10 15 20 25

Roll

angl

e (de

g)

minus9

minus8

minus7

minus6

minus5

minus4

minus3

minus2

minus1

0


Time (s) Time (s)5 10 15 20 25

Pitc

h an

gle (

deg

)

00 50

1

2

3

4

5


INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25

Yaw

angl

e (de

g)

minus2

0

2

4

6

8

10


5


050


minus1minus15

Trajectory

minus2minus200

Distance Y (m)

2040

15

1

0

2

05

25

60

Dist

ance

Z (m

)




References























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










(b)

(c)

(d)(e)(f)

(a)


HC12MPU6050 PX4FLOW



5 Conclusion





Velo

city

(ms

)

Velo

city

(ms

)

Velo

city

(ms

)

minus02

0

02

04

06

08

1

INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25minus05

0

05

1

15

2

25

3

35

4

10 15 20 25minus08

minus06

minus04

minus02

0

02

04

06

08


5 0 5 0 5


10 15 20 25

Dist

ance

(m)

minus2

minus15

minus1

minus05

0

05


INS onlyEKF

INS onlyEKF

INS onlyEKF


minus10

0

10

20

30

40

50


10 15 20 25

Dist

ance

(m)

Dist

ance

(m)

0

05

1

15

2


50 5 0 5





Acknowledgments



Time (s)0 10 15 20 25

Roll

angl

e (de

g)

minus9

minus8

minus7

minus6

minus5

minus4

minus3

minus2

minus1

0


Time (s) Time (s)5 10 15 20 25

Pitc

h an

gle (

deg

)

00 50

1

2

3

4

5


INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25

Yaw

angl

e (de

g)

minus2

0

2

4

6

8

10


5


050


minus1minus15

Trajectory

minus2minus200

Distance Y (m)

2040

15

1

0

2

05

25

60

Dist

ance

Z (m

)




References























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation











Velo

city

(ms

)

Velo

city

(ms

)

Velo

city

(ms

)

minus02

0

02

04

06

08

1

INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25minus05

0

05

1

15

2

25

3

35

4

10 15 20 25minus08

minus06

minus04

minus02

0

02

04

06

08


5 0 5 0 5


10 15 20 25

Dist

ance

(m)

minus2

minus15

minus1

minus05

0

05


INS onlyEKF

INS onlyEKF

INS onlyEKF


minus10

0

10

20

30

40

50


10 15 20 25

Dist

ance

(m)

Dist

ance

(m)

0

05

1

15

2


50 5 0 5





Acknowledgments



Time (s)0 10 15 20 25

Roll

angl

e (de

g)

minus9

minus8

minus7

minus6

minus5

minus4

minus3

minus2

minus1

0


Time (s) Time (s)5 10 15 20 25

Pitc

h an

gle (

deg

)

00 50

1

2

3

4

5


INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25

Yaw

angl

e (de

g)

minus2

0

2

4

6

8

10


5


050


minus1minus15

Trajectory

minus2minus200

Distance Y (m)

2040

15

1

0

2

05

25

60

Dist

ance

Z (m

)




References























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation










Time (s)0 10 15 20 25

Roll

angl

e (de

g)

minus9

minus8

minus7

minus6

minus5

minus4

minus3

minus2

minus1

0


Time (s) Time (s)5 10 15 20 25

Pitc

h an

gle (

deg

)

00 50

1

2

3

4

5


INS onlyEKF

INS onlyEKF

INS onlyEKF

10 15 20 25

Yaw

angl

e (de

g)

minus2

0

2

4

6

8

10


5


050


minus1minus15

Trajectory

minus2minus200

Distance Y (m)

2040

15

1

0

2

05

25

60

Dist

ance

Z (m

)




References























RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation





















RoboticsJournal of





Journal of



RotatingMachinery





VLSI Design



Shock and Vibration







Journal of



Volume 2014


SensorsJournal of





Propagation









Research Article Optical Flow Sensor/INS/Magnetometer...

Documents

Transcript of Research Article Optical Flow Sensor/INS/Magnetometer...