Mobile Sensing VI Orientation Estimation. Spring 2015 Petteri Nurmi

Transkriptio

1 Mobile Sensing VI Orientation Estimation Spring 2015 Petteri Nurmi

2 Learning Objectives Understand how the orientation of device can be characterized and represented Familiarize with the most common formalisms for orientation: Euler angles, Quaternions and Directed Cosine Matrices Understand sensor fusion techniques: complementary and Kalman filtering Magnetic sensing, magnetometer basics and error sources, example applications for the magnetometer

3 Attitude Estimation and Tracking Attitude refers to the orientation of an object with respect to a given reference frame (Lecture V) Estimation: determine current orientation of device Requires calibration or noise estimation to compensate for different error sources Tracking: estimate orientation over time Requires sensor fusion, i.e., combining measurements from different sensors Previous lecture looked at forces that characterize motion along the reference frame This lecture looks at how to align measurements of different sensors along the reference frame

4 Earth s Magnetic Field Magnetic field extending from the Earth s interior to where it meets the solar wind magnetic dipole that is tilted about 10 degrees from Earth s rotational axis Varies over time, but sufficiently slowly to enable compass-based navigation Strength of the field expressed in micro Teslas (μt) (or gauss 1G = 100 μt) Between 22 μt to 67 μt depending on position Geomagnetic institutes provide detailed maps of the strength of the field at different locations

5 Earth s Magnetic Field Helsinki: Intensity: μt Declination: 8 29 Inclination: Magnetic field described by direction and magnitude (or strength) Direction expressed in terms of inclination and declination è magnetic field is a 3D vector Inclination (magnetic dip) Angle made with the horizontal by the Earth s magnetic lines Positive values indicate field is pointing downwards, negative upwards: -90 at south pole, 0 at equator, +90 at north pole Declination Angle between magnetic north and true north Varies depending on position along the Earth s surface, as well as over time

6 Global Reference Frame Global reference frame defined by Earth s magnetic field and gravity Gravity vector (0, 0, g) Magnetic field vector B * (cos σ, 0, sin σ) where B is the strength of the magnetic field and σ is the inclination Attitude estimation defined as the problem of aligning the local coordinates of the device with the global reference frame As discussed, device frame often separate from the user s reference frame Practical note: attitude estimation requires COARSE location information to determine the magnetic field parameters

7 Sensors: Magnetometer Sensor that measures the strength and direction of magnetic field in the current environment Many ways to measure the strength of the field Most common ways (magnetoresistance and Hall effect) measure changes in electric properties Sensitivity Determines the maximal possible accuracy that can be achieved with the sensor, e.g., 0.5 μt 3 Range: should be at least ±1000 μt Main uses Electronic compasses Object detection (e.g., military originally used for submarine detection)

8 Magnetometer Noise: Hard and Soft Iron Noise Theoretically, magnetic field measurements in a single location should form a perfect circle as device is rotated Permanently magnetized ferromagnetic components cause a so-called hard iron offset on magnetometers Magnets, speakers, or any other ferromagnetic objects Results in an additive bias in the magnetometer, i.e., the centre of the circle shifts Soft iron Caused by materials that distort the magnetic field, but that do not generate their own field Depends on the orientation of the material relative to the sensor and the field Perturbs the circle into an ellipse

9 Representing Attitude Attitude estimation can be seen as coordinate transformation that aligns device with global reference Transformations between coordinate systems are defined through rotations Different ways to represent the rotations Euler angles: rotation represented as a matrix that captures rotations relative to individual coordinate axes Quaternions: rotations represented as hypercomplex numbers and spherical coordinates Direction cosines: rotations represented as directional cosines that capture the angles between coordinate axes

10 Euler Angles yaw Euler s rotation theorem: any rotation can be described using three angles Yaw or heading ψ: rotation around the z-axis Pitch θ: rotation around the x-axis Roll φ: rotation around the y-axis Each rotation can be expressed in matrix form (see left) Overall rotation can be expressed as a product of the individual orientations Only meaningful is the order of the rotations is defined We follow x-convention R = R x R y R z Other conventions equally valid, but result in different rotation matrix Accordingly, we can align the device with global reference frame if the three angles can be estimated

11 Gimbal Lock Gimbal is a ring that is suspended so that it can rotate about an axis Euler angles assume changes in object s orientation behave like three gimbals aligned around different axes Gimbal lock Loss of one degree of freedom when axes of two gimbals are in parallel configuration Example 1: device is pointing up è yaw and roll axes parallel and cannot be distinguished Can also occur between yaw and pitch (device sideways) Fourth gimbal (or dimension) required to avoid problems

12 Quaternions Quaternions provide an alternative representation for describing rotations Quaternion defined as a 4-tuple (q 0, q 1, q 2, q 3 ) Consists of scalar part q 0 and vector part q = (q 1, q 2, q 3 ), Form of hyper-complex numbers, with a real (scalar) and imaginary component q = q 0 + q 1 i + q 2 j + q 3 k If q 0 = 0, q is a pure quaternion Quaternion algebra Main rule (Hamilton s equation): i 2 = j 2 = k 2 = ijk = -1 Product rule: pq = p 0 q 0 p q + p 0 q + q 0 p + p x q Complex conjugate: q* = q 0 -q 1 i -q 2 j -q 3 k Any quaternion with norm equal to one, i.e., q = 1, is called an unit quaternion

13 Quaternions The importance of quaternions stems from their relationship with rotations of vectors Any unit quaternion can be written in the form: q = q 0 + q = cos θ + q / q sin θ For any unit quaternion q and for any vector v, the operation L q (v) = q*vq can be interpreted as rotation of v through an angle 2θ about q as the axis of orientation The orientation/attitude of an object can be expressed as an unit quaternion and coordinate transformations can be achieved by rotating the quaternion As quaternions rely on four-dimensional representation, they are not vulnerable to gimbal lock

14 Quaternions and Euler Angles Given an unit quaternion q = (q 0, q 1, q 2, q 3 ), the orthogonal matrix resulting from quaternion rotation is: The quaternion can converted to Euler angles using the following equations:

15 Direction Cosine Matrix Direction cosine generally refers to the angle θ between two vectors u and v From basic linear algebra: cos θ = u v / u v In attitude estimation, direction cosines represent angles of basis vectors between coordinate systems Let e = (e i, e j, e k ) and n = (n i, n j, n k ) denote the basis vectors of two coordinate systems Define c jk = e j n k and C = [c ij ] u = Cv corresponds to representation of u in coordinate system defined by basis n Direction cosine matrices thus represent rotations through a change of basis vectors

16 Estimating Pitch and Roll Recall that pitch defined as rotation around x-axis è gravity orthogonal to the pitch axis in global coordinate frame accelerometer (and gyroscope) can be used to estimate the gravity component of the device (see Lecture V) g d = (g x, g y, g z ) denotes the gravity estimate of the device g denotes the global gravity = (0, 0, g) Pitch and roll can then be estimated by aligning the two gravity estimates (in Euler angles): g d = Rg è (g x, g y, g z ) / g d = (-sin θ, sin φ cos θ, cos θ cos φ) tan θ = (g y /g z ) tan φ = (-g x / (g y 2 + g z2 ))

17 Example: Detecting Shifts in Orientation left bottom top right Switching display orientation is one of the first use cases for tilt (and roll) sensing Can be implemented using a state machine that compares components of gravity and switches orientation whenever dominant axis changes General case: shifts in orientation can be detected by monitoring changes in pitch and roll angles Whenever combined effect exceeds a threshold, assume device orientation changed I.e., (Δ θ + Δ φ ) > ε

18 Example Application: Placement Estimation Generally pitch angles are relatively unique across different device placements Device placement can be (in many cases) identified by examining pitch-related features Pose classification (Park et al., 2012) Treat placement detection as a classification problem, and use pitch related features as input Pitch feature vector: x G = ( g x, g y, g z, (g x 2 + g y2 ), (g x 2 + g z2 ), (g y 2 + g z2 )) Four placements: bag/pocket/hand/ear SVM (and C4.5) classifiers with pitch features Performance for similar classes (e.g., bag and pocket) close to 90%, for others over 95% Accuracy depends on gravity estimation quality

19 Yaw/Heading Estimation In the absence of errors, heading of the device can be solved from magnetometer measurements using: ψ = atan(h x /h y ) + D Where h x and h y are magnetometer readings and D is the declination at current location Two sources of noise: Device orientation not aligned with Earth reference frame Measurements need to be projected, or rotated, into reference frame Tilt compensation Hard and soft iron errors require calibration to be accurate Common technique is to rotate the magnetometer around different axes and to estimate shape of field In practice can be performed during sensor fusion by using complementary information from accelerometer

20 Tilt Compensation Estimating heading from magnetometer measurements requires compensating for tilt Recall that Earth s magnetic field given by the vector: B r = B * (cos σ, 0, sin σ) Tilt compensation de-rotates magnetometer measurements B p with the estimated pitch and roll R z B p = R y R x B r It can be shown that the yaw angle of the device is then given by tan ψ = -B c y / B c x where B c y and B c x are derotated and noise compensated magnetometer values Practical notes: pitch and yaw usually solved with a function ATAN2 that is restricted to -180, 180, and roll with a function ATAN that is restricted to range -90,

21 Yaw Estimation with Hard and Soft Iron Offsets Hard and soft iron offsets can be estimated as part of attitude estimation process In the absence of noise, we have: B p = R z R y R x B r (B p device measurement, B r global reference vector) Hard iron V causes additive offset: B p = R z R y R x B r + V Soft iron W causes multiplicative effect: B p = W R z R y R x B r + V It can be shown that B c = R y R x W -1 (B r - V) In other words, yaw can be estimated as before as long as the hard and soft iron offsets are removed first

22 Sensor Fusion Sensor Fusion refers to the combination of sensor data from multiple (disparate) sources Attitude estimation relies on fusion of 2-4 sensors: Inertial: accelerometer + gyroscope MARG: inertial + magnetometer MARG + barometer if altitude differences need to be separated Several algorithms, but basic idea same: Find optimal rotation to align device reference frame with global frame while compensating for noise

23 Filtering Basics Filtering algorithms generally consist of two stages Propagation: estimating next values of system state Filtering: correcting estimates based on measurements Propagation step requires a model of system dynamics which is difficult to obtain for attitude Instead, filtering techniques for attitude estimation use gyroscope to simulate system dynamics Accelerometer, gyroscope, and possibly magnetometer and other sensors used as long-term reference, i.e., provide the basis for the filtering step

24 Complementary Filtering Complementary filtering complements orientation estimates of one sensor with those of another Gyroscope highly sensitive but suffers from drift in the long-term è good for capturing short-term changes Accelerometer and magnitude accurate but have latency è good for long-term changes (Basic) Complementary Filter: θ = (1 - α) * GyroScope + α * AccMagOrientation Effectively performs low-pass filtering on accelerometer and magnetometer and high-pass filtering on gyroscope Typically 0.01 α

25 Direction Cosine Matrix (DCM) Filter DCM is a sensor fusion algorithm that is based on direction cosine representation of rotation Basic idea similar to complementary filtering, but uses nonlinear form of rotation tracking Propagation step performed using gyroscope Constraints on direction cosines used to reduce drift Filtering step performed using correction vectors that are applied on gyroscope signals Yaw compensation: GPS or magnetometer Pitch & roll compensation: accelerometer + gyroscope

26 DCM: Gyroscope Change in orientation given by: dr(t)/dt = ω(t) x r(t) Rotation rate ω(t) estimated using gyroscope ω(t) = ω gyro (t) + ω correction (t) Rotation vector r(t) tracked using numerical integration r(t) = r(0) + r(τ) x dθ(τ) The vectors r are direction cosines, i.e., q = Rv defines the rotation to global reference frame Implemented using the following propagation equation: r(t + dt) = r(t) + r(t) x dθ(t) d θ(t) = ω(t) dt Matrix form: R(t + dt) = R(t)Ω(t) where

27 DCM: Basis Correction and Renormalization Direction cosines assume the underlying vector space as orthogonal basis vectors Drift accumulation from gyroscope can result in orthogonality constraints being violated Renormalization applied to enforce orthogonality and compensate drift over time Basic idea is to calculate dot product of the column matrices and pull them apart equally by their error Orthogonality è dot product should be equal to zero Basis vectors should also be of unit length Normalization applied after error correction

28 Madgwick MARG Filter Quaternion based approach that treats rotation alignment as optimization problem Recall quaternion rotation operator: L q (v) = q*vq Goal then is to find a quaternion q so that L q (v) provides best possible fit with global coordinate frame Specifically: min q f(q,s,v) = q*vq s where s is an Earth reference vector Optimization problem solved using (iterative) gradient descent: Real-time tracking support provided through sensor fusion with gyroscope

29 Madgwick MARG Filter Gyroscope: Similarly to DCM, gyroscope measurements accumulated to estimates using numerical integration Sensor fusion A variant of complementary filter which uses a coefficient that adapts to current environment Gyroscope drift and magnetometer noise compensation also applied as part of the process; see original article (Madwick et al. 2011)

30 Kalman Filtering Transition matrix of the system dynamics Measurement model matrix Kalman filter is a closed-form solution for an optimal Bayes filter under linear and Gaussian equations Results in closed form equations, derivation of them out-of-scope; please see [Trawny & Roumeliotis, 2005] Uses following general form: x k = A k-1 x k-1 + q k y k = H k x k + r k White process noise N(0,Q) White measurement noise N(0,R) p(x k, x k-1 ) = N(x k A k-1 x k-1, Q) p(y k, x k ) = N(y k H k x k, R)

31 Kalman Filter PREDICT FILTER 1. Project system state X* k A k-1 X k-1 + Bu k-1 2. Project error covariance P* k A k-1 P k-1 A T k-1 + Q 1. Computer Kalman gain 2. Update estimate with measurement 3. Update error covariance Gyroscope Accelerometer Magnetometer

32 EKF Filtering In the context of attitude estimation, a so-called extended Kalman filter (EKF) is required Based on quaternions, rotation quaternion q and bias vectors of sensors define system state Non-linear model, assume state transitions and observations given by differentiable models Gyroscope measurement model ω gyro (t) = ω (t) + b + n gyro Here ω (t) is true angular velocity, b is gyroscope bias, and n gyro is a noise term Propagation step updates system state with current gyroscope values

33 EKF Filtering: Propagation Step 1. Propagate gyroscope bias 2. Estimate new turn rate as the difference of gyroscope measurement and estimated bias 3. Propagate quaternion q using the current and propagated gyroscope estimates 4. Compute state transition matrix Φ using a small angle approximation of gyroscope error Assess variance of numerical integration within a small time interval Noise covariance Q d estimated analogously 5. Compute state covariance matrix using EKF equation:

34 EKF Filter: Update Step 1. Form differentiated measurement matrix H Based on differential of the quaternion rotation 2. Compute measurement residuals R 3. Compute covariance of residual 4. Compute Kalman gain 5. Use Kalman gain to calculate a correction term 6. Update parameters Quaternion Gyro bias estimate Re-estimate turn rate, and covariance

35 Summary 1/2 Mobile devices in arbitrary orientations è attitude estimation required to align sensor measurements Magnetometer, accelerometer and gyroscope Yaw estimation using magnetometer Tilt (pitch and roll) compensation achieved using accelerometers Accuracy depends on the robustness of gravity estimation, gyroscope can be used to assist there Alignment seen as a coordinate transformation task, which can be solved by finding appropriate rotation Magnetometer (or GPS) required for solving heading Vulnerable to many sources of noise, calibration/filtering required to ensure reliability of measurements Can be used to support also other applications, e.g., positioning and object detection

36 Summary 2/2 Different ways to represent rotations: Euler angles: yaw, pitch, roll Quaternions: hyper-complex numbers Direction cosines: cosines that transform vector basis In continuous tracking, sensor fusion required to increase reliability of estimates Complementary filtering: linear combination of gyroscope and magnetometer + acc. estimates DCM filtering: simple to implement non-linear gyro tracking Madgwick: Quaternion-based optimization Kalman filtering: generic technique which provides closed form equations for filtering

37 References Kuipers, J. B., Quaternions and Rotation Sequences, Geometry, Integrability and Quantization, Coral Press, 2000, Pedley, M., Tilt Sensing Using a Three-Axis Accelerometer, Freescale Semiconductor Application Note, 2013 Ozyagcilar, T., Implementing a Tilt-Compensated ecompass using Accelerometer and Magnetometer Sensors, Freescale Semiconductor Application Note AN428, 2012 Park, Jun-geun.; Patel, A.; Curtis, D.; Ledlie, J. & Teller, S., Online Pose Classification and Walking Speed Estimation using Handheld Devices, Proceedings of the 14th ACM International Conference on Ubiquitous Computing (Ubicomp), 2012 Madgwick, S. O. H.; Harrison, A. J. L. & Vaidyanathan, R., Estimation of IMU and MARG orientation using a gradient descent algorithm, Proceedings of the IEEE International Conference on Rehabilitation Robotics, IEEE, 2011, 1-7 Trawny, N. & Roumeliotis, S. I., Indirect Kalman Filter for 3D Attitude Estimation Department of Computer Science & Engineering, University of Minnesota, 2005 Sabatini, A. M., Quaternion-Based Extended Kalman Filter for Determining Orientation by Inertial and Magnetic Sensing, IEEE Transactions on biomedical engineering, 2006, 53,

38 References Lenz, J. & Edelstein, A. S., Magnetic Sensors and their applications, Sensors, 2006, 6, Chung, J.; Donahoe, M.; Schmandt, C.; Kim, I.-J.; Razavai, P. & Wiseman, M. Indoor location sensing using geo-magnetism, Proceedings of the 9th International Conference on Mobile Systems, Applications, and Services (MobiSys), ACM, 2011, Eisenman, S. B.; Miluzzo, E.; Lane, N. D.; Peterson, R. A.; Ahn, G.-S. & Campbell, A. T., BikeNet: A mobile sensing system for cyclist experience mapping, TOSN, 2009, 6 Caruso, M. J., Applications of Magnetic Sensors for Low Cost Compass Systems, Proceedings of the IEEE Position Location and Navigation Symposium, IEEE, 2000, Li, B.; Gallagher, T.; Dempster, A. G. & Rizos, C., How feasible is the use of magnetic field alone for indoor positioning?, Proceedings of the 2012 International Conference on Indoor Positioning and Indoor Navigation (IPIN), 2012 Skvortzov, V. Y.; Lee, H.-K.; Bang, S. & Lee, Y., Application of Electronic Compass for Mobile Robot in an Indoor Environment, Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), IEEE, 2007,