Inertial Navigation and Various Applications of Inertial Data. Yongcai Wang. 9 November 2016

Transcription

1 Inertial Navigation and Various Applications of Inertial Data Yongcai Wang 9 November 2016

2 Types of Gyroscope Mechanical Gyroscope Laser Gyroscope Sagnac Effect

3 Stable Platform IMU and Strapdown IMU In Stable platform, the inertial sensors are mounted on a platform which is isolated from any external rotational motion, returning readings in global frame. In strapdown systems the inertial sensors are mounted rigidly onto the device, returning readings in body frame.

4 Types of Gyroscopes: MEMS Coriolis force F=2mv*w

5 Accuracy Comparison GG1320AN (Laser) MPU6050 (MEMS) Size 88*88*45 5*5*5 Power 1.6Watt 5V, 3.6mA Angular random walk o / h <0.2 Bias stability o / h <70 o / Price > o / h h

6 Inertial Navigation System

7 Inertial Navigation Systems Gyroscope provides angle velocity in body frame Accelerometer provides acceleration in body frame 1. Pose Update 2. Calculate linear ACC 3. Update velocity Gravity g 4. Update position

8 Outline Pose update method Velocity and position update Error correction Step segmentation Pedestrian dead reckoning

9 Coordinate Frame Rotating Skew Symmetric matrix ω= x y z Angular velocity: ( ω, ω, ω ) The derivative of a time-varying rotation matrix: 0 wz wy S( ω) = wz 0 w wy wx 0 x

10 Coordinate Frame Rotating By first order Taylor explanation T δts( ω) = Rt ( + δ t) Rt () I 33 How the rotation matrix change according to the angular rate Derivate the angular change from the rotation matrix How to derive the angular change from the rotation matrix

11 Continuous Rotation Infinitesimal Rotating is Commutive R xyz =rotx(0.001)*roty(0.002)*rotz(0.003) R yxz =roty(0.002)*rotx(0.002)*rotz(0.003) R xyz =R yxz Recovery of the infinitesimal rotation angle vex(r xyz )=vex(r yxz ) =(0.001, 0.002, 0.003) Difference between two infinitesimally poses Incremental displacement Incremental rotation

12 1. Pose update (1) 1. Pose update by first order Taylor approximation: 2. Pose update by higher order approximation: Let When is small:

13 1. Pose update (2) Let, it is easy to verify that

14 Orthogonarize and Normalization R(t) may becomes not orthonormal If the added term is small, the result will be close to orthonormal and we can straighten it up Normalization should be carried out after each step of integration

15 2. Pose Update by Quaternions It become easier when using unit-quaternions. The derivative of q: Integration of quaternion rate: Normalization

16 3. Velocity and Location Update Convert the acceleration to global frame Exclude the gravity Double integral to calculate velocity and position

17 MEMS gyroscope error sources bias Random noise calibration Temperature effects

18 INS Example

19 INS Error Correction Using external measurements to correct the states of IMU. Zero Velocity Update (ZUPT) GPS, Other locating system Using a Kalman Filter to correct an INS based on external measurements. Tracking full state of INS (openshoe) Tracking state error of INS

20 INS Error Correction The true state of an IMU at time t The solution calculated by INS The error state = ( Φ,, ) T T T T gx gy gz xt t vt p t in which δφ t = ( δφt, δφt, δφt ) δ δ δ δ Relation of Error state and IMU State: R = Φ Rˆ t t t v = vˆ + δ v t t t p = pˆ + δ p t t t 1 δφ δφ gz gy t t gz gx t δφt 1 δφt gy gx δφt δφt 1 Φ =

21 INS Error Correction by KF External measurement Estimated State Prediction Error State Correction ( T T T) δx = δφ, δv, δp t t t t T R = Φ Rˆ t t t v = vˆ + δ v t t t p = pˆ + δ p t t t INS System Reset δ x t to zero Working in parallel, without much changing to the INS system

22 Error State Prediction State transition matrix I 0 0 g Ft = δtsa ( t ) I 0 0 δti I where gz 0 at at Sa ( t ) = at 0 at gy gx at at 0 Error State Prediction gz g gz gx δxˆ ˆ t = Ftδxt δ t T P = FP F + Q t t t δ t t t Eric Foxlin Founder of intersense Oliver Woodman In Google Android group [1] Pedestrian tracking with shoe-mounted inertial sensors, E. Foxlin 2005, IEEE computer graphics and applications. [2] Pedestrian localisation for indoor environments, Oliver Woodman, 2010, PhD thesis, Cambridge university

23 Error State Prediction Q t Tt 0 0 = 0 Ut The noise item in state prediction ( ) T ( ) 2 T = δt Rˆ W Rˆ W t t t t t bx 2 ( σ ) 0 0 w by 2 = 0 ( σ w ) 0 bz ( σ w ) Representing noises of gyroscope ( ) T ( ) 2 U = δt Rˆ A Rˆ t t t t A t bx 2 ( σ ) 0 0 a by 2 = 0 ( σ a ) 0 bz ( σ a ) Representing noises of accelerometer

24 Error State Correction External measurement In a simple test by keeping the IMU on table ZUPT Ultrasound z = h( x ) + v t t t t h( x ) = v t t t Error State Correction H t ht( xt) = x xˆ K= PH ( HPH + R) t T T 1 t t t t t t t δ xˆ = K ( z h( xˆ )) t t t t t P= ( I KH) P t t t t Ht = ( 0, I,0) R t 2 σ σ 2 = 2 σ

25 KF-based correction vs. naïve correction

26 Pedestrian Dead Reckoning Zero Velocity detection: ZUPT ZUPT Estimated State Prediction Error State Correction T T T ( ) δx = δφ, δv, δp t t t t T R = Φ Rˆ t t t v = vˆ + δ v t t t p = pˆ + δ p t t t Reset δ x t to zero INS System

27 Step Segmentation In practice few systems or applications require updates at such a high frequency. Step events need to be detected for collaborating with other filtering algorithms. Parameterize a step event by: e = ( l, δ z, δθ, ξ ) Step length t t t t t Height change Heading change Offset between heading and step direction

28 Step Segmentation: PDR filter Step events are generated at the end of each stance phase.

29 INS vs. PDR Filter

30 Some Evaluation Results in [2]

31 2.5D view

32 Other Related works Using particle filters to correct PDR using map constraints [woodman 2010]. Using magnetometers to correct PDR [zhou 2014, mobicom] Using WiFi radiomap to correct PDR [Miyazaki 2014, Ubicomp]

33 Other cognitive problems using IMU State recognition

34 Activities

35 Process of Activity Recogonition

36 Data collection

37 Preprocessing

38 Time domain Feature Extraction

39 Frequency domain features

40 Classification Base-level Classification Meta-level Classification

41 Base level classifier Decision Tree Decision Table K-Nearest Neighbor(KNN) Hidden Markov Model(HMM) Support Vector Machine(SVM) Naïve Bayes, Artificial Neural Networks, Gaussian Mixture Model, etc

42 Voting Meta-level classifier Each base level classifier gives a vote, the class label receiving the most votes is the final decision. Stacking Learning algorithm to learn how to combine the predictions of the base-level classifiers. Cascading Iterative process to combine base-level classifiers. Sub-optimal compare to others

43 Applications Calorie consumption estimation Fall detection Daily life logging Sport training Gait analysis for healthcare

44 Cognitive problems using IMU Arm tracking by smart watch 5DOF model of Arm [3] I am a Smartwatch and I can Track my User s S. Shen, Mobicom 2016

45 Arm tracking by smart watch Point cloud model of elbow and whist

46 Home Work 1. Develop an PDR filter (Step Segmentation) based on the openshoe project. Compare the tracking accuracy of Step-based and INS-based Test the algorithms using self-collected IMU data traces.