Apparatuses and methods for calibrating magnetometer attitude-independent parameters
Methods and apparatuses for calibrating attitude-independent parameters of a 3-D magnetometer are provided. A calibration method includes storing and updating data related to a N×9 matrix T and a N×1 matrix U extended for each measurement with an additional row and an additional element, respectively, the additional row and the additional element being calculated based on values measured by the 3-D magnetometer for the respective measurement. The method further includes calculating analytically (1) a symmetric non-orthogonal 3×3 matrix D representing scaling and skew of the 3-D magnetometer measured values and (2) a vector b representing bias of the 3-D magnetometer measured values, using the stored data and a singular value decomposition (SVD) method.
1. A method for calibrating attitude-independent parameters of a 3-D magnetometer, the method comprising:
obtaining a magnetic field reading in each of three orthogonal axes, B x , B y , B z , at each of N different times from a 3-D magnetometer;
storing and updating a N×9 matrix, T, and a N×1 matrix, U, wherein each element in each row of matrix T comprises 9 different mathematical functions of B x , B y , B z , at the corresponding time and each row of matrix U comprises a tenth mathematical function of B x , B y , B z , at the corresponding time; and
calculating analytically (1) a symmetric non-orthogonal 3×3 matrix D representing scaling and skew of the 3-D magnetometer readings and (2) a 3×1 matrix b representing bias of the 3-D magnetometer readings, B x , B y , B z , using the stored data and a singular value decomposition method.
2. The method of claim 1 wherein matrix T comprises:
[
[
B
x
2
+
B
y
2
-
2
B
z
2
B
x
2
-
2
B
y
2
+
B
z
2
2
B
x
·
B
y
2
B
x
·
B
z
2
·
B
y
·
B
z
-
2
B
x
-
2
B
y
-
2
B
z
1
]
1
…
[
B
x
2
+
B
y
2
-
2
B
z
2
B
x
2
-
2
B
y
2
+
B
z
2
2
B
x
·
B
y
2
B
x
·
B
z
2
·
B
y
·
B
z
-
2
B
x
-
2
B
y
-
2
B
z
1
]
N
]
and matrix U comprises:
[
-
(
B
x
2
+
B
y
2
+
B
z
2
)
1
…
-
(
B
x
2
+
B
y
2
+
B
z
2
)
N
]
.
3. The method of claim 1 wherein the calculating comprises:
computing a vector K=(T T ×T) −1 ×T T ×U;
generating a matrix F using elements of the vector K, and determining a matrix G, where F=G×G, using a singular value decomposition method;
determining a scale offset bias {tilde over (b)}=G −1 ×[K(6)K(7)K(8)] T ,
determining a scale factor
co
=
H
2
[
K
(
6
)
K
(
7
)
K
(
8
)
]
×
F
-
1
×
[
K
(
6
)
K
(
7
)
K
(
8
)
]
T
-
K
(
9
)
,
where |H| 2 is a square of local geomagnetic field strength;
calculating the vector b=sqrt(co)·{tilde over (b)} and matrix pD=sqrt(co)·G; and
extracting the symmetric non-orthogonal 3×3 matrix D=pD−I 3×3 ,
wherein K(i) represents the i th element of vector K.
4. The method of claim 3 wherein
F
=
[
1
+
K
(
1
)
+
K
(
2
)
K
(
3
)
K
(
4
)
K
(
3
)
1
+
K
(
1
)
-
2
K
(
2
)
K
(
5
)
K
(
4
)
K
(
5
)
1
-
2
K
(
1
)
+
K
(
2
)
]
.
5. The method of claim 1 wherein a 9×9 matrix L is used to accumulate T T ×T and a 9×1 matrix M is used to accumulate T T ×U, where L and M are updated for each measurement according to
T N =[ B x 2 +B y 2 −2 B z 2 B x 2 −2 B y 2 +B z 2 2 B x ·B y 2 B x ·B z 2· B y ·B z −2 B x −2 B y −2 B z 1] N
U N =( B x 2 +B y 2 +B Z 2 ) N
L N+1 =L N +( T N+1 T ×T N+1 ) and
M N+1 =M N +( T N+1 T ×U N+1 ).
6. The method of claim 1 further comprising estimating accuracy of determining the symmetric non-orthogonal 3×3 matrix D and the vector b.
7. The method of claim 6 wherein the accuracy of determining the symmetric non-orthogonal 3×3 matrix D and the vector b is estimated by computing matrices
Y
1
=
[
0
0
0
2
3
pD
11
0
-
2
3
pD
33
2
3
pD
12
0
-
2
3
pD
23
0
0
0
2
3
pD
11
-
2
3
pD
22
0
0
2
3
pD
13
-
2
3
pD
23
0
0
0
pD
12
pD
12
0
pD
11
+
pD
22
pD
23
pD
13
0
0
0
pD
13
0
pD
13
pD
23
pD
11
+
pD
33
pD
12
0
0
0
0
pD
23
pD
23
pD
13
pD
12
pD
22
+
pD
33
pD
11
pD
12
pD
13
b
x
0
0
b
y
b
z
0
pD
12
pD
22
pD
23
0
b
y
0
b
x
0
b
z
pD
13
pD
23
pD
33
0
0
b
z
0
b
x
b
y
2
b
x
2
b
y
2
b
z
0
0
0
0
0
0
]
Y
2
=
K
×
[
0
0
0
4
3
pD
11
4
3
pD
22
4
3
pD
33
2
pD
12
2
pD
13
2
pD
23
]
∂
K
∂
J
=
1
co
·
(
Y
1
-
Y
2
)
L
N
+
1
=
L
N
+
(
T
N
+
1
T
×
T
N
+
1
)
and
P
KK
=
σ
z
2
·
L
-
1
;
where σ z 2 =12·|H| 2 ·σ 2 +6·σ 4 ; and
σ is a constant standard deviation of a transformed measurement noise vector of the 3-D magnetometer;
calculating an error covariance matrix
P
JJ
=
(
∂
K
∂
J
)
-
1
×
P
KK
×
(
∂
K
∂
J
)
-
1
;
and
computing error of the elements of b and D as elements of
J =[ b x b y b z pD 11 pD 22 pD 33 pD 12 pD 13 pD 23 ] T
according to
ε J =sqrt(diag( P JJ ))
wherein
pD
=
I
3
×
3
+
D
=
[
pD
11
pD
12
pD
13
pD
12
pD
22
pD
23
pD
13
pD
23
pD
33
]
,
K
=
(
T
T
×
T
)
-
1
×
T
T
×
U
co
=
H
2
[
K
(
6
)
K
(
7
)
K
(
8
)
]
×
F
-
1
×
[
K
(
6
)
K
(
7
)
K
(
8
)
]
T
-
K
(
9
)
where |H| 2 is a square of local geomagnetic field strength; and
K(i) represents the i th element of vector K.
8. The method of claim 1 further comprising:
determining if the 3-D magnetometer measured values meet a predetermined criterion; and
using the 3-D magnetometer measured values in the updating of the matrices T and U only if they meet the predetermined criterion.
9. An apparatus configured to perform a calibration of attitude-independent parameters of a 3-D magnetometer, comprising:
an interface configured to receive field values measured on three axes by the 3-D magnetometer, B x , B y , B z ; and
a data processing unit configured:
to store and update data related to a N×9 matrix, T, and a N×1 matrix, U, wherein each element in each row of matrix T comprises 9 different mathematical functions of B x , B y , B z , at the corresponding time and each row of matrix U comprises a tenth mathematical function of B x , B y , B z , at the corresponding time; and
to calculate analytically (1) a symmetric non-orthogonal 3×3 matrix D representing scaling and skew of the 3-D magnetometer measured values and (2) a 3×1 matrix b representing bias of the 3-D magnetometer measured values due to attitude-independent factors in each of the three dimensions, using the stored data and a singular value decomposition (SVD) method.
10. The apparatus of claim 9 wherein matrix T comprises:
[
[
B
x
2
+
B
y
2
-
2
B
z
2
B
x
2
-
2
B
y
2
+
B
z
2
2
B
x
·
B
y
2
B
x
·
B
z
2
·
B
y
·
B
z
-
2
B
x
-
2
B
y
-
2
B
z
1
]
1
…
[
B
x
2
+
B
y
2
-
2
B
z
2
B
x
2
-
2
B
y
2
+
B
z
2
2
B
x
·
B
y
2
B
x
·
B
z
2
·
B
y
·
B
z
-
2
B
x
-
2
B
y
-
2
B
z
1
]
N
]
and matrix U comprises:
[
-
(
B
x
2
+
B
y
2
+
B
z
2
)
1
…
-
(
B
x
2
+
B
y
2
+
B
z
2
)
N
]
.
11. The apparatus of claim 9 wherein the data processing unit is configured to calculate the symmetric non-orthogonal 3×3 matrix D and the vector b by:
(1) computing a vector K=(T T ×T) −1 ×T T ×U;
(2) generating a matrix F using elements of the vector K, and determining a matrix G, where F=G×G, using a singular value decomposition method;
(3) determining a scale offset bias {tilde over (b)}=G −1 ×[K(6)K(7)K(8)] T ,
(4) determining a scale factor
co
=
H
2
[
K
(
6
)
K
(
7
)
K
(
8
)
]
×
F
-
1
×
[
K
(
6
)
K
(
7
)
K
(
8
)
]
T
-
K
(
9
)
where |H| 2 is a square of local geomagnetic field strength;
(5) calculating the vector b=sqrt(co)·{tilde over (b)} and matrix pD=sqrt(co)·G; and
(6) extracting the symmetric non-orthogonal 3×3 matrix D=pD−I 3×3 ,
wherein K(i) represents the i th element of vector K.
12. The apparatus of claim 11 where
F
=
[
1
+
K
(
1
)
+
K
(
2
)
K
(
3
)
K
(
4
)
K
(
3
)
1
+
K
(
1
)
-
2
K
(
2
)
K
(
5
)
K
(
4
)
K
(
5
)
1
-
2
K
(
1
)
+
K
(
2
)
]
.
13. The apparatus of claim 9 wherein the data processing unit is configured to use a 9×9 matrix L to accumulate T T ×T, and a 9×1 matrix M to accumulate T T ×U, wherein M and L are updated for each measurement according to
T N =[ B x 2 +B y 2 −2 B z 2 B x 2 −2 B y 2 +B z 2 2 B x ·B y 2 B x ·B z 2· B y ·B z −2 B x −2 B y −2 B z 1] N
U N =( B x 2 +B y 2 +B Z 2 ) N
L N+1 =L N +( T N+1 T ×T N+1 ) and
M N+1 =M N +( T N+1 T ×U N+1 ).
14. The apparatus of claim 9 wherein the data processing unit is further configured to estimate accuracy of determining the symmetric non-orthogonal 3×3 matrix D and the vector b.
15. The apparatus of claim 14 further wherein the data processing unit estimates accuracy of determining the symmetric non-orthogonal 3×3 matrix D and the vector b by computing matrices
Y
1
=
[
0
0
0
2
3
pD
11
0
-
2
3
pD
33
2
3
pD
12
0
-
2
3
pD
23
0
0
0
2
3
pD
11
-
2
3
pD
22
0
0
2
3
pD
13
-
2
3
pD
23
0
0
0
pD
12
pD
12
0
pD
11
+
pD
22
pD
23
pD
13
0
0
0
pD
13
0
pD
13
pD
23
pD
11
+
pD
33
pD
12
0
0
0
0
pD
23
pD
23
pD
13
pD
12
pD
22
+
pD
33
pD
11
pD
12
pD
13
b
x
0
0
b
y
b
z
0
pD
12
pD
22
pD
23
0
b
y
0
b
x
0
b
z
pD
13
pD
23
pD
33
0
0
b
z
0
b
x
b
y
2
b
x
2
b
y
2
b
z
0
0
0
0
0
0
]
Y
2
=
K
×
[
0
0
0
4
3
pD
11
4
3
pD
22
4
3
pD
33
2
pD
12
2
pD
13
2
pD
23
]
∂
K
∂
J
=
1
co
·
(
Y
1
-
Y
2
)
L
N
+
1
=
L
N
+
(
T
N
+
1
T
×
T
N
+
1
)
and
P
KK
=
σ
z
2
·
L
-
1
;
where σ z 2 =12·|H| 2 ·σ 2 +6·σ 4 ; and
σ is a constant standard deviation of a transformed measurement noise vector of the 3-D magnetometer;
calculating an error covariance matrix
P
JJ
=
(
∂
K
∂
J
)
-
1
×
P
KK
×
(
∂
K
∂
J
)
-
1
;
and
computing error of the elements of b and D as elements of
J =[ b x b y b z pD 11 pD 22 pD 33 pD 12 pD 13 pD 23 ] T
according to
ε J =sqrt(diag( P JJ ))
wherein
pD
=
I
3
×
3
+
D
=
[
pD
11
pD
12
pD
13
pD
12
pD
22
pD
23
pD
13
pD
23
pD
33
]
,
K
=
(
T
T
×
T
)
-
1
×
T
T
×
U
co
=
H
2
[
K
(
6
)
K
(
7
)
K
(
8
)
]
×
F
-
1
×
[
K
(
6
)
K
(
7
)
K
(
8
)
]
T
-
K
(
9
)
where |H| 2 is a square of local geomagnetic field strength; and
K(i) represents the i th element of vector K.
16. The apparatus of claim 9 wherein the data processing unit is further configured to:
determine if the 3-D magnetometer measured values meet a predetermined criterion; and
use the 3-D magnetometer measured values in the updating of the matrices T and U only if they meet the predetermined criterion.
17. A computer readable medium storing executable code, which, when executed by a processor, causes the processor to execute a method for calibrating attitude-independent parameters of a 3-D magnetometer, the method comprising:
obtaining a magnetic field reading in each of three orthogonal axes, B x , B y , B z , at each of N different times from a 3-D magnetometer;
storing and updating a N×9 matrix, T, and a N×1 matrix, U, wherein each element in each row of matrix T comprises 9 different mathematical functions of B x , B y , B z , at the corresponding time and each row of matrix U comprises a tenth mathematical function of B x , B y , B z , at the corresponding time; and
calculating analytically (1) a symmetric non-orthogonal 3×3 matrix D representing scaling and skew of the 3-D magnetometer readings and (2) a 3×1 matrix b representing bias of the 3-D magnetometer readings due to attitude-independent factors in each of the three dimensions, using the stored data and a singular value decomposition method.
18. The computer readable medium of claim 17 wherein matrix T comprises:
[
[
B
x
2
+
B
y
2
-
2
B
z
2
B
x
2
-
2
B
y
2
+
B
z
2
2
B
x
·
B
y
2
B
x
·
B
z
2
·
B
y
·
B
z
-
2
B
x
-
2
B
y
-
2
B
z
1
]
1
…
[
B
x
2
+
B
y
2
-
2
B
z
2
B
x
2
-
2
B
y
2
+
B
z
2
2
B
x
·
B
y
2
B
x
·
B
z
2
·
B
y
·
B
z
-
2
B
x
-
2
B
y
-
2
B
z
1
]
N
]
and matrix U comprises:
[
-
(
B
x
2
+
B
y
2
+
B
z
2
)
1
…
-
(
B
x
2
+
B
y
2
+
B
z
2
)
N
]
.
19. The computer readable medium of claim 17 wherein the calculating comprises:
computing a vector K=(T T ×T) −1 ×T T ×U;
generating a matrix F using elements of the vector K, and determining a matrix G, where F=G×G, using a singular value decomposition method;
determining a scale offset bias {tilde over (b)}=G −1 ×[K(6)K(7)K(8)] T ;
determining a scale factor
co
=
H
2
[
K
(
6
)
K
(
7
)
K
(
8
)
]
×
F
-
1
×
[
K
(
6
)
K
(
7
)
K
(
8
)
]
T
-
K
(
9
)
,
where |H| 2 is a square of local geomagnetic field strength;
calculating the vector b=sqrt(co)·{tilde over (b)} and matrix pD=sqrt(co)·G; and
extracting the symmetric non-orthogonal 3×3 matrix D=pD−I 3×3 ,
wherein K(i) represents the i th element of vector K.
20. The computer readable medium of claim 17 wherein the method further comprises estimating accuracy of determining the symmetric non-orthogonal 3×3 matrix D and the vector b.