Calibration of a magnetometer in combination with inertial sensors

Transcription

1 Calibration of a magnetometer in combination with inertial sensors Manon Kok, Linköping University, Sweden Joint work with: Thomas Schön, Uppsala University, Sweden Jeroen Hol, Xsens Technologies, the Netherlands Fredrik Gustafsson, Linköping University, Sweden Henk Luinge, Xsens Technologies, the Netherlands March th 205

2 Summary of our work on magnetometer calibration Manon Kok, Jeroen Hol, Thomas Schön, Fredrik Gustafsson and Henk Luinge, Calibration of a magnetometer in combination with inertial sensors. The 5th International Conference on Information Fusion, Singapore, July 202. Manon Kok and Thomas Schön, Maximum likelihood calibration of a magnetometer using inertial sensors. Proceedings of the 9th World Congress of the International Federation of Automatic Control (IFAC), Cape Town, South Africa, August 204. Manon Kok and Thomas Schön, Magnetometer calibration using inertial sensors, 204. Paper A in: Manon Kok, Probabilistic modeling for positioning applications using inertial sensors, 204 (Licentiate thesis). Manon Kok Magnetometer calibration March th / 2

3 Introduction: sensors Magnetometers Manon Kok Magnetometer calibration March th / 2

4 Introduction: sensors Magnetometers Accelerometers Gyroscopes Inertial sensors Manon Kok Magnetometer calibration March th / 2

5 Introduction: sensors Magnetometers Accelerometers Gyroscopes Inertial sensors Inertial sensors and magnetometers are widely used for orientation estimation. Manon Kok Magnetometer calibration March th / 2

6 Introduction: magnetometer calibration Magnetometers can be used in combination with inertial sensors to estimate the sensor s orientation provided that the magnetometer is properly calibrated. Manon Kok Magnetometer calibration March th / 2

7 Introduction: magnetometer calibration Magnetometers can be used in combination with inertial sensors to estimate the sensor s orientation provided that the magnetometer is properly calibrated. The magnetometer needs to be calibrated for. magnetometer sensor errors, 2. presence of magnetic material rigidly attached to the sensor, 3. misalignment between magnetometer and inertial sensor axes. Manon Kok Magnetometer calibration March th / 2

8 Introduction: magnetometer calibration The magnetometer needs to be calibrated for. magnetometer sensor errors, 2. presence of magnetic material rigidly attached to the sensor, 3. misalignment between magnetometer and inertial sensor axes. Manon Kok Magnetometer calibration March th / 2

9 Introduction: magnetometer calibration data Rotate the IMU obtain a sphere of magnetometer data 0 y m,t = Rt bn m n 0 0 Manon Kok Magnetometer calibration March th / 2

10 Introduction: magnetometer calibration data Rotate the IMU obtain a sphere of magnetometer data 0 y m,t = Rt bn m n 0 0 Manon Kok Magnetometer calibration March th / 2

11 Introduction: magnetometer calibration data Uncalibrated magnetometer data will have an offset (o) 0 y m,t = Rt bn m n + o 0 0 Manon Kok Magnetometer calibration March th / 2

12 Introduction: magnetometer calibration data Uncalibrated magnetometer data will have an offset (o) 0 y m,t = Rt bn m n + o 0 0 Manon Kok Magnetometer calibration March th / 2

13 Introduction: magnetometer calibration data 0 Uncalibrated magnetometer data will have an offset (o) and it will be scaled (D) y m,t = DRt bn m n + o 0 0 Manon Kok Magnetometer calibration March th / 2

14 Introduction: magnetometer calibration data 0 Uncalibrated magnetometer data will have an offset (o) and it will be scaled (D) y m,t = DRt bn m n + o 0 0 Manon Kok Magnetometer calibration March th / 2

15 Introduction: magnetometer calibration data 0 Uncalibrated magnetometer data will have an offset (o) and it will be scaled, skewed (D) y m,t = DRt bn m n + o 0 0 Manon Kok Magnetometer calibration March th / 2

16 Introduction: magnetometer calibration data 0 Uncalibrated magnetometer data will have an offset (o) and it will be scaled, skewed (D) y m,t = DRt bn m n + o 0 0 Manon Kok Magnetometer calibration March th / 2

17 Introduction: magnetometer calibration data 0 Uncalibrated magnetometer data will have an offset (o) and it will be scaled, skewed and rotated (D). y m,t = DRt bn m n + o 0 0 Manon Kok Magnetometer calibration March th / 2

18 Introduction: magnetometer calibration data 0 Uncalibrated magnetometer data will have an offset (o) and it will be scaled, skewed and rotated (D). y m,t = DRt bn m n + o 0 0 Manon Kok Magnetometer calibration March th / 2

19 Magnetometer calibration algorithm Magnetometer calibration algorithm. Rigidly mount the magnetometer and the inertial sensors on the platform to be used. 2. Rotate the assembly to collect inertial and magnetometer measurements. Manon Kok Magnetometer calibration March th / 2

20 Magnetometer calibration algorithm Magnetometer calibration algorithm. Rigidly mount the magnetometer and the inertial sensors on the platform to be used. 2. Rotate the assembly to collect inertial and magnetometer measurements. Uncalibrated magnetometer measurements Wrong orientation estimates Manon Kok Magnetometer calibration March th / 2

21 Magnetometer calibration algorithm Magnetometer calibration algorithm. Rigidly mount the magnetometer and the inertial sensors on the platform to be used. 2. Rotate the assembly to collect inertial and magnetometer measurements. Uncalibrated magnetometer measurements Wrong orientation estimates 3. Estimate the calibration parameters. Obtain an initial estimate of the parameters. 2. Obtain a maximum likelihood estimate of the parameters. Manon Kok Magnetometer calibration March th / 2

22 Magnetometer calibration algorithm Magnetometer calibration algorithm. Rigidly mount the magnetometer and the inertial sensors on the platform to be used. 2. Rotate the assembly to collect inertial and magnetometer measurements. Uncalibrated magnetometer measurements Wrong orientation estimates 3. Estimate the calibration parameters. Obtain an initial estimate of the parameters. 2. Obtain a maximum likelihood estimate of the parameters. 4. Use the estimated calibration parameters together with the magnetometer measurements. Manon Kok Magnetometer calibration March th / 2

23 Magnetometer calibration algorithm Magnetometer calibration algorithm. Rigidly mount the magnetometer and the inertial sensors on the platform to be used. 2. Rotate the assembly to collect inertial and magnetometer measurements. Uncalibrated magnetometer measurements Wrong orientation estimates 3. Estimate the calibration parameters. Obtain an initial estimate of the parameters. 2. Obtain a maximum likelihood estimate of the parameters. 4. Use the estimated calibration parameters together with the magnetometer measurements. Calibrated magnetometer measurements Correct orientation estimates Manon Kok Magnetometer calibration March th / 2

24 Introduction: magnetometer calibration data 0 Uncalibrated magnetometer data will have an offset (o) and it will be scaled, skewed and rotated (D). y m,t = DRt bn m n + o 0 0 Manon Kok Magnetometer calibration March th / 2

25 Summary of our work on magnetometer calibration Manon Kok, Jeroen Hol, Thomas Schön, Fredrik Gustafsson and Henk Luinge, Calibration of a magnetometer in combination with inertial sensors. The 5th International Conference on Information Fusion, Singapore, July 202. Manon Kok and Thomas Schön, Maximum likelihood calibration of a magnetometer using inertial sensors. Proceedings of the 9th World Congress of the International Federation of Automatic Control (IFAC), Cape Town, South Africa, August 204. Manon Kok and Thomas Schön, Magnetometer calibration using inertial sensors, 204. Paper A in: Manon Kok, Probabilistic modeling for positioning applications using inertial sensors, 204 (Licentiate thesis). Manon Kok Magnetometer calibration March th / 2

26 Contributions Derived a magnetometer calibration algorithm by solving a maximum likelihood problem. Validated the algorithm using experimental data showing that it works and leads to improved heading estimates even in the presence of large disturbances. Performed an identifiability analysis to quantify how much rotation that is needed to be able to solve the calibration problem. Manon Kok Magnetometer calibration March th / 2

27 Contributions Derived a magnetometer calibration algorithm by solving a maximum likelihood problem. Validated the algorithm using experimental data showing that it works and leads to improved heading estimates even in the presence of large disturbances. Performed an identifiability analysis to quantify how much rotation that is needed to be able to solve the calibration problem. Manon Kok Magnetometer calibration March th / 2

28 Problem formulation State-space model with unknown states (x t ) and parameters (θ) x t+ = f t (x t, u t, θ) + B(x t )v t (θ) y t = h t (x t, θ) + e t (θ) Manon Kok Magnetometer calibration March th / 2

29 Problem formulation State-space model with unknown states (x t ) and parameters (θ) x t+ = f t (x t, u t, θ) + B(x t )v t (θ) y t = h t (x t, θ) + e t (θ) Dynamic model: the state x t represents the sensor s orientation, Manon Kok Magnetometer calibration March th / 2

30 Problem formulation State-space model with unknown states (x t ) and parameters (θ) x t+ = f t (x t, y ω,t, θ) + B(x t )v ω,t (θ) y t = h t (x t, θ) + e t (θ) Dynamic model: the state x t represents the sensor s orientation, the gyroscope measurements (y ω,t ) are used as input to the dynamic model. Manon Kok Magnetometer calibration March th / 2

31 Problem formulation State-space model with unknown states (x t ) and parameters (θ) x t+ = f t (x t, y ω,t, θ) + B(x t )v ω,t (θ) y t = h t (x t, θ) + e t (θ) Measurement model: Manon Kok Magnetometer calibration March th / 2

32 Problem formulation State-space model with unknown states (x t ) and parameters (θ) x t+ = f t (x t, y ω,t, θ) + B(x t )v ω,t (θ) y a,t = R bn t (at n g n ) + e a,t Rt bn g n + e a,t Measurement model: accelerometer measurement model Manon Kok Magnetometer calibration March th / 2

33 Problem formulation State-space model with unknown states (x t ) and parameters (θ) x t+ = f t (x t, y ω,t, θ) + B(x t )v ω,t (θ) y a,t = R bn t (at n g n ) + e a,t Rt bn g n + e a,t Measurement model: accelerometer measurement model assumes that the sensor s acceleration is approximately zero. Manon Kok Magnetometer calibration March th / 2

34 Problem formulation State-space model with unknown states (x t ) and parameters (θ) x t+ = f t (x t, y ω,t, θ) + B(x t )v ω,t (θ) y a,t = R bn t (at n g n ) + e a,t Rt bn g n + e a,t y m,t = DRt bn m n + o + e m,t Measurement model: accelerometer measurement model assumes that the sensor s acceleration is approximately zero. magnetometer measurement model Manon Kok Magnetometer calibration March th / 2

35 Problem formulation State-space model with unknown states (x t ) and parameters (θ) x t+ = f t (x t, y ω,t, θ) + B(x t )v ω,t (θ) y a,t = R bn t (at n g n ) + e a,t Rt bn g n + e a,t y m,t = DRt bn m n + o + e m,t Measurement model: accelerometer measurement model assumes that the sensor s acceleration is approximately zero. magnetometer measurement model assumes that the magnetometer measurements can be calibrated using a calibration matrix D and offset vector o Manon Kok Magnetometer calibration March th / 2

36 Problem formulation State-space model with unknown states (x t ) and parameters (θ) x t+ = f t (x t, y ω,t, θ) + B(x t )v ω,t (θ) y a,t = R bn t (at n g n ) + e a,t Rt bn g n + e a,t y m,t = DRt bn m n + o + e m,t where y ω,t = ω t + b ω + e ω,t e ω,t N (0, Σ ω ) e a,t N (0, Σ a ) e m,t N (0, Σ m ) Manon Kok Magnetometer calibration March th / 2

37 Maximum likelihood estimate Compute a maximum likelihood estimate of the unknown parameters, θ ML = arg max θ Θ p θ (y :N ) Manon Kok Magnetometer calibration March th 205 / 2

38 Maximum likelihood estimate Compute a maximum likelihood estimate of the unknown parameters, θ ML = arg max θ Θ arg min θ Θ p θ (y :N ) 2 N t= y t ŷ t t (θ) 2 St (θ) + log det S t(θ) Manon Kok Magnetometer calibration March th 205 / 2

39 Maximum likelihood estimate Compute a maximum likelihood estimate of the unknown parameters, θ ML = arg max θ Θ arg min θ Θ p θ (y :N ) 2 N t= y t ŷ t t (θ) 2 S t (θ) + log det S t(θ) where ŷ t t and S t are the predicted measurement and the residual covariance from an extended Kalman filter. Manon Kok Magnetometer calibration March th 205 / 2

40 Initial estimate The maximum likelihood problem is non-convex and needs proper initialization. θ = {D, o, m n, δ ω, Σ ω, Σ a, Σ m } Manon Kok Magnetometer calibration March th / 2

41 Initial estimate The maximum likelihood problem is non-convex and needs proper initialization. θ = {D, o, m n, δ ω, Σ ω, Σ a, Σ m }. Obtain initial sensor characteristics. Manon Kok Magnetometer calibration March th / 2

42 Initial estimate The maximum likelihood problem is non-convex and needs proper initialization. θ = {D, o, m n, δ ω, Σ ω, Σ a, Σ m }. Obtain initial sensor characteristics. 2. Ellipse fitting (to a unit sphere) Manon Kok Magnetometer calibration March th / 2

43 Initial estimate The maximum likelihood problem is non-convex and needs proper initialization. θ = {D, o, m n, δ ω, Σ ω, Σ a, Σ m }. Obtain initial sensor characteristics. 2. Ellipse fitting (to a unit sphere) Manon Kok Magnetometer calibration March th / 2

44 Initial estimate The maximum likelihood problem is non-convex and needs proper initialization. θ = {D, o, m n, δ ω, Σ ω, Σ a, Σ m }. Obtain initial sensor characteristics. 2. Ellipse fitting (to a unit sphere). 3. Determine the rotation of the ellipse to align the magnetometer and inertial sensor axes Manon Kok Magnetometer calibration March th / 2

45 Initial estimate The maximum likelihood problem is non-convex and needs proper initialization. θ = {D, o, m n, δ ω, Σ ω, Σ a, Σ m }. Obtain initial sensor characteristics. 2. Ellipse fitting (to a unit sphere). 3. Determine the rotation of the ellipse to align the magnetometer and inertial sensor axes Manon Kok Magnetometer calibration March th / 2

46 Magnetometer calibration algorithm Magnetometer calibration algorithm. Rigidly mount the magnetometer and the inertial sensors on the platform to be used. 2. Rotate the assembly to collect inertial and magnetometer measurements. Uncalibrated magnetometer measurements Wrong orientation estimates 3. Estimate the calibration parameters. Obtain an initial estimate of the parameters. 2. Obtain a maximum likelihood estimate of the parameters. 4. Use the estimated calibration parameters together with the magnetometer measurements. Calibrated magnetometer measurements Correct orientation estimates Manon Kok Magnetometer calibration March th / 2

47 Experimental setup Manon Kok Magnetometer calibration March th / 2

48 Experimental setup Inertial measurement unit (inertial sensors + magnetometer) Manon Kok Magnetometer calibration March th / 2

49 Experimental setup Inertial measurement unit (inertial sensors + magnetometer) Magnetic disturbance Manon Kok Magnetometer calibration March th / 2

50 Experimental setup Inertial measurement unit (inertial sensors + magnetometer) Magnetic disturbance The IMU is placed in a block that can be put in orientations differing 90 degrees from each other. Manon Kok Magnetometer calibration March th / 2

51 Experimental results Manon Kok Magnetometer calibration March th / 2

52 Experimental results 0 y m,t = DRt bn m n + o + e m,t D = , ô = Manon Kok Magnetometer calibration March th / 2

53 Experimental results 0 Norm magnetic field [a.u.] Before calibration After calibration Time [s] The norm of the calibrated magnetometer measurements is around, i.e. the data is properly mapped to a unit sphere. Manon Kok Magnetometer calibration March th / 2

54 Resulting orientation (heading) error Calibrated mag Time [s] Heading error in degrees after calibration for 90 turns z up z down x up x down y down Manon Kok Magnetometer calibration March th / 2

55 Conclusions Developed a magnetometer calibration algorithm which calibrates a magnetometer in combination with inertial sensors to obtain better orientation estimates. It corrects for magnetometer sensor errors, presence of magnetic material rigidly attached to the sensor, misalignment between magnetometer and inertial sensor axes. We applied the algorithm to real data, showing that improved heading estimates are obtained. Manon Kok Magnetometer calibration March th / 2

56 An optimization-based approach to human body motion capture using inertial sensors Manon Kok, Jeroen D. Hol 2 and Thomas B. Schön 3 Linköping University, Sweden 2 Xsens Technologies, the Netherlands 3 Uppsala University, Sweden March 3th 205

57 Inertial motion capture Manon Kok Human body inertial motion capture March 3th / 2

58 Inertial motion capture 7 inertial measurement units containing: Accelerometers Inertial sensors Gyroscopes Magnetometers Manon Kok Human body inertial motion capture March 3th / 2

59 Inertial motion capture 7 inertial measurement units containing: Accelerometers Inertial sensors Gyroscopes Magnetometers The magnetic field at the different sensor locations is typically different. Manon Kok Human body inertial motion capture March 3th / 2

60 Inertial motion capture 7 inertial measurement units containing: Accelerometers Inertial sensors Gyroscopes Magnetometers + Biomechanical model The magnetic field at the different sensor locations is typically different. Manon Kok Human body inertial motion capture March 3th / 2

61 Inertial motion capture 7 inertial measurement units containing: Accelerometers Inertial sensors Gyroscopes Magnetometers + Biomechanical model Assuming that the body segments are connected to each other, the relative position and orientation of the body is observable (if the subject is not standing completely still). Manon Kok Human body inertial motion capture March 3th / 2

62 Problem formulation We solve the motion capture problem by solving a maximum a posteriori (MAP) problem min log p(x y ) log p(θ) z={x :N,θ} }{{} initial state + prior N log p(x t x t, θ) t=2 } {{ } dynamic model s.t. c bio (z) = 0 }{{} biomechanical model N log p(y t x t, θ) t= }{{} biomechanical/sensor model Manon Kok Human body inertial motion capture March 3th / 2

63 Problem formulation We solve the motion capture problem by solving a maximum a posteriori (MAP) problem min log p(x y ) log p(θ) z={x :N,θ} }{{} initial state + prior N log p(x t x t, θ) t=2 } {{ } dynamic model s.t. c bio (z) = 0 }{{} biomechanical model N log p(y t x t, θ) t= }{{} biomechanical/sensor model x :N : time-varying states such as the sensor positions, velocities and orientations, the body segment positions and orientations. θ: constant model parameters such as sensor biases. y :N : inertial measurements. Manon Kok Human body inertial motion capture March 3th / 2

64 Problem formulation We solve the motion capture problem by solving a maximum a posteriori (MAP) problem min log p(x y ) log p(θ) z={x :N,θ} }{{} initial state + prior N log p(x t x t, θ) t=2 } {{ } dynamic model s.t. c bio (z) = 0 }{{} biomechanical model N log p(y t x t, θ) t= }{{} biomechanical/sensor model c bio (z): constraints imposed by the biomechanical model. Manon Kok Human body inertial motion capture March 3th / 2

65 Problem formulation We solve the motion capture problem by solving a maximum a posteriori (MAP) problem min log p(x y ) log p(θ) z={x :N,θ} }{{} initial state + prior N log p(x t x t, θ) t=2 } {{ } dynamic model s.t. c bio (z) = 0 }{{} biomechanical model N log p(y t x t, θ) t= }{{} biomechanical/sensor model c bio (z): constraints imposed by the biomechanical model. A constrained nonlinear least-squares problem. Manon Kok Human body inertial motion capture March 3th / 2

66 Problem formulation We solve the motion capture problem by solving a maximum a posteriori (MAP) problem min log p(x y ) log p(θ) z={x :N,θ} }{{} initial state + prior N log p(x t x t, θ) t=2 } {{ } dynamic model s.t. c bio (z) = 0 }{{} biomechanical model N log p(y t x t, θ) t= }{{} biomechanical/sensor model c bio (z): constraints imposed by the biomechanical model. A constrained nonlinear least-squares problem. Solve this as a batch problem using standard solvers. Manon Kok Human body inertial motion capture March 3th / 2

67 Biomechanical model - strategy The body segments are connected at the joints. Manon Kok Human body inertial motion capture March 3th / 2

68 Biomechanical model - strategy The body segments are connected at the joints. constraint Manon Kok Human body inertial motion capture March 3th / 2

69 Biomechanical model - strategy The body segments are connected at the joints. constraint The position and orientation of the sensors on the body is approximately constant. Manon Kok Human body inertial motion capture March 3th / 2

70 Biomechanical model - strategy The body segments are connected at the joints. constraint The position and orientation of the sensors on the body is approximately constant. objective function Manon Kok Human body inertial motion capture March 3th / 2

71 Biomechanical model - strategy The body segments are connected at the joints. constraint The position and orientation of the sensors on the body is approximately constant. objective function Some joints are restricted in their rotational freedom (optional). Manon Kok Human body inertial motion capture March 3th / 2

72 Biomechanical model - strategy The body segments are connected at the joints. constraint The position and orientation of the sensors on the body is approximately constant. objective function Some joints are restricted in their rotational freedom (optional). objective function Manon Kok Human body inertial motion capture March 3th / 2

73 Biomechanical model - strategy The body segments are connected at the joints. constraint The position and orientation of the sensors on the body is approximately constant. objective function Some joints are restricted in their rotational freedom (optional). objective function The positions and orientations of the sensors on the body are assumed to be known but it is possible to extend the algorithm to estimate these as well. Manon Kok Human body inertial motion capture March 3th / 2

74