APPARATUSES AND METHODS FOR MAGNETOMETER ALIGNMENT CALIBRATION WITHOUT PRIOR KNOWLEDGE OF THE LOCAL MAGNETIC FIELD
Apparatuses and methods calibrate attitude dependent magnetometer alignment parameters of a magnetometer mounted together with other angular position sensors on a device without prior knowledge of the local magnetic field and allowing a constant but unknown offset of the yaw angle in the reference attitudes with respect to an earth-fixed coordinate system. The method includes acquiring magnetic field measurements from the magnetometer and corresponding estimated angular positions subject to an unknown yaw offset relative to a gravitational reference system. The method further includes iteratively computing a scale and vector components of a quaternion representing a misalignment matrix, an inclination angle of local magnetic field, and a yaw angle offset, using an extended Kalman filter (EKF) infrastructure with a specific designed model and constraints, based on the magnetic field measurements and the corresponding estimated angular positions.
1 . A method for calibrating attitude dependent magnetometer alignment parameters of a magnetometer mounted together with other angular position sensors on a device, the method comprising:
acquiring magnetic field measurements from the magnetometer and corresponding estimated angular positions subject to an unknown yaw offset relative to a gravitational reference system; and
iteratively computing a scale and vector components of a quaternion representing a misalignment matrix, an inclination angle of local magnetic field, and a yaw angle offset using an extended Kalman filter (EKF) infrastructure with a specific designed model and constraints, based on the magnetic field measurements and the corresponding estimated angular positions.
2 . The method of claim 1 , wherein computing the EKF comprises, in each iteration,
predicting an error covariance as a sum of error covariance matrix at a previous step and an error covariance matrix of a process model of the EKF;
calculating a difference between a normalized measurement and an observation model of the EKF;
calculating a Kalman gain using (1) a Jacobian matrix of partial derivatives of the observation model with respect to a current state of the EKF, (2) the predicted error covariance and (3) a magnetometer noise covariance;
performing state correction using the calculated Kalman gain and the predicted error covariance;
performing an error covariance correction using the Kalman gain and the Jacobian matrix of partial derivatives of the observation model with respect to a current state of the EKF;
normalizing the quaternion; and
limiting the inclination angle to be between
-
π
2
and
π
2
,
and the initial yaw angle offset to be between −π and π.
3 . The method of claim 2 , wherein the error covariance matrix of the process model of EKF is updated dynamically by multiplying a baseline constant matrix with
a first factor depending on an angle difference between estimated misalignment angles between of a current system state and of a system state obtained from an accuracy verification algorithm, and
a second factor that depends on a magnitude of a change in the estimated angular position.
4 . The method of claim 3 , wherein the first factor is
1 if the angle difference is larger than a predetermined threshold,
α×the angle difference if the angle difference is larger than 1, and
α otherwise, wherein α is a non-negative constant much smaller than 1.
5 . The method of claim 3 , wherein the second factor is a factor decaying being multiplied with a fixed quantity less than 1 if a difference between angular positions determined at successive steps is less than a predetermined threshold, and it is set to 1 if the difference between the angular positions determined at successive steps is larger than the predetermined threshold.
6 . The method of claim 1 , wherein the computing of the EKF is reduced to a Wahba problem.
7 . The method of claim 6 , wherein the Wahba problem is solved using singular value decomposition.
8 . The method of claim 7 , further comprising solving the Wahba problem using a method different from SVD for accuracy measurement.
9 . The method of claim 1 , wherein the iteratively computing of the scale and the vector components of the quaternion stops when a difference between angles determined in successive iterations becomes less than a predetermined threshold or when after a predetermined number of iterations.
10 . An apparatus configured to perform a calibration of attitude-dependent magnetometer alignment parameters of a magnetometer mounted together with other angular position sensors on a device, comprising:
an interface configured to receive magnetic field measurements and corresponding estimated angular positions subject to an unknown yaw offset of the device relative to a gravitational reference system; and
a data processing unit configured to iteratively compute a scale and vector components of a quaternion representing a misalignment matrix, an inclination angle of local magnetic field, and a yaw angle offset using an extended Kalman filter infrastructure with a specific designed model and constraints, based on the magnetic field measurements and the corresponding estimated angular positions.
11 . The apparatus of claim 10 , wherein the data processing unit is configured to perform, for each iteration,
predicting an error covariance as a sum of error covariance matrix at a previous step and an error covariance matrix of a process model of the EKF;
calculating a difference between a normalized measurement and an observation model of the EKF;
calculating a Kalman gain using (1) a Jacobian matrix of partial derivatives of the observation model with respect to a current state of the EKF, (2) the predicted error covariance and (3) a magnetometer noise covariance;
performing state correction using the calculated Kalman gain and the predicted error covariance;
performing an error covariance correction using the Kalman gain and the Jacobian matrix of partial derivatives of the observation model with respect to a current state of the EKF;
normalizing the quaternion; and
limiting the inclination angle to be between
-
π
2
and
π
2
,
and the initial yaw angle offset to be between −π and π.
12 . The apparatus of claim 10 , wherein the data processing unit is configured to update dynamically the error covariance matrix of the process model of EKF by multiplying a baseline constant matrix with
a first factor depending on an angle difference between estimated misalignment angles between of a current system state and of a system state obtained from an accuracy verification algorithm, and
a second factor that depends on a magnitude of a change in the estimated angular position.
13 . The apparatus of claim 12 , wherein the first factor is
1 if the angle difference is larger than a predetermined threshold,
α×the angle difference if the angle difference is larger than 1, and
is α otherwise, wherein α is a non-negative constant much smaller than 1.
14 . The apparatus of claim 12 , wherein the second factor is a factor decaying being multiplied with a fixed quantity less than 1 if a difference between angular positions determined at successive steps less than a predetermined threshold, and it is set to 1 if the difference between the angular positions determined at successive steps is larger than the predetermined threshold.
15 . The apparatus of claim 10 , wherein the data processing unit is configured to reduce the computing of the EKF to a Wahba problem.
16 . The apparatus of claim 15 , wherein the data processing unit is configured to solve the Wahba problem using singular value decomposition.
17 . The apparatus of claim 16 , wherein the data processing unit is configured to perform an accuracy measurement by solving the Wahba problem using a method different from SVD.
18 . The apparatus of claim 16 , wherein the data processing unit is configured to stop iteratively computing the scale and the vector components of the quaternion when a difference between angles determined in successive iterations becomes less than a predetermined threshold or when after a predetermined number of iterations.
19 . A computer readable medium storing executable codes which when executed by a processor make the processor execute a method calibrating attitude dependent magnetometer alignment parameters of a magnetometer mounted together with other angular position sensors on a device, the method comprising:
acquiring magnetic field measurements from the magnetometer and corresponding estimated angular positions subject to an unknown yaw offset relative to a gravitational reference system; and
iteratively computing a scale and vector components of a quaternion representing a misalignment matrix, an inclination angle of local magnetic field, and a yaw angle offset using an extended Kalman filter infrastructure with a specific designed model and constraints, based on the magnetic field measurements and the estimated angular position.
20 . The computer readable medium of claim 19 , wherein computing the EKF comprises, in each iteration:
predicting an error covariance as a sum of error covariance matrix at a previous step and an error covariance matrix of a process model of the EKF;
calculating a difference between a normalized measurement and an observation model of the EKF;
calculating a Kalman gain using (1) a Jacobian matrix of partial derivatives of the observation model with respect to a current state of the EKF, (2) the predicted error covariance and (3) a magnetometer noise covariance;
performing state correction using the calculated Kalman gain and the predicted error covariance;
performing an error covariance correction using the Kalman gain and the Jacobian matrix of partial derivatives of the observation model with respect to a current state of the EKF;
normalizing the quaternion; and
limiting the inclination angle to be between
-
π
2
and
π
2
,
and the initial yaw angle offset to be between −π and π.
21 . The computer readable medium of claim 20 , wherein the error covariance matrix of the process model of EKF is updated dynamically by multiplying a baseline constant matrix with
a first factor depending on an angle difference between estimated misalignment angles between of a current system state and of a system state obtained from an accuracy verification algorithm, and
a second factor that depends on a magnitude of a change in the estimated angular position.
22 . The computer readable medium of claim 21 , wherein the first factor is
1 if the angle difference is larger than a predetermined threshold,
α*the angle difference if the angle difference is larger than 1, and
is α otherwise, wherein α is a non-negative constant much smaller than 1.
23 . The computer readable medium of claim 21 , wherein the second factor is a factor decaying being multiplied with a fixed quantity less than 1 if a difference between angular positions determined at successive steps less than a predetermined threshold, and it is set to 1 if the difference between the angular positions determined at successive steps is larger than the predetermined threshold.
24 . The computer readable medium of claim 19 , wherein the computing of the EKF is reduced to a Wahba problem.
25 . The computer readable medium of claim 24 , wherein the Wahba problem is solved using singular value decomposition.
26 . The computer readable medium of claim 25 , further comprising solving the Wahba problem using a method different from SVD for accuracy measurement.
27 . The computer readable medium of claim 19 , wherein the iteratively computing of the scale and the vector components of the quaternion stops when a difference between angles determined in successive iterations becomes less than a predetermined threshold or when after a predetermined number of iterations.