# Tom Judd <<<>>> Math

## For Compass Calibrations

### Invert

Most solutions for compass calibrations make use of a matrix inversion routine. Fortunately, these matrices are well behaved provided you have a good set of data. You have to have good data in order to calibrate a precision compass. If it blows up on you then it is a good indication that you have bad data.

I first ran into matrix inversion techniques in a numerical analysis course given in FORTRAN about 50 years ago. In the context of solving a set of linear equations one operates on a matrix using Gaussian elimination until it is in lower triangular form, then with back substitution, starting at the bottom, you work your way up, always having one equation and one unknown. Well, I didn't want to write any back substitution code, so I just continued zeroing out the upper triangle too. If you apply the same operations to an identity matrix, then the product of all those operations is the inverse. Magic!

Most matrix inversion routines want you to pivot, i.e. swap rows, so you don't divide by a small number. I dispense with that here with the expectation that all our diagonal elements are of reasonable size

With no pivoting, and no back-substitution, the code gets really simple. The end product consists of two matrices: the original matrix which has been reduced to an identity matrix, and the inversion matrix, which started out as an identity matrix. You can probably write it up yourself in C without peeking once you have read through this once. So let's start.

```
[1.1  ,0.1, 0.05],
[0.1  ,1.2, 0.2 ],
[0.05 ,0.2, 1.3 ]

```
Shown in augmented form with the identity matrix appended to the end it is
``` 1.100  0.100  0.050  1.000  0.000  0.000
0.100  1.200  0.200  0.000  1.000  0.000
0.050  0.200  1.300  0.000  0.000  1.000
```
We first normalize the first diagonal element (divide the row by the diagonal element)
```   1.0000  0.0909  0.0455
0.1000  1.2000  0.2000
0.0500  0.2000  1.3000
```
In the code, the part that does the normalization is shown below. Notice that we check for zero before dividing. The code applies the division to all elements in the row AND all elements of the augmented portion of the matrix. Some algorithms work on a single augmented matrix and span 2N elements. I think this way is clearer.
```
alpha = A[ii][ii]
if abs(alpha) < epsilon: return (Ainv,-1)

for jj in range(N):
A[ii][jj] =  A[ii][jj]/alpha
Ainv[ii][jj] =  Ainv[ii][jj]/alpha
```
We then add a scaled version of it to the first element we want to eliminate. Since it is 0.1 we multiply the first row by -0.1 and add it to the second row. We get
```   1.0000  0.0909  0.0455
0.0000  1.1909  0.1955
0.0500  0.2000  1.3000
```
The augmented part, which started out as an identity matrix, now reads
```   0.9091  0.0000  0.0000
-0.0909  1.0000  0.0000
0.0000  0.0000  1.0000
```
In the code, the part that does the elimination is shown below.
```
beta = A[kk][ii]
for jj in range(N):
A[kk][jj]=A[kk][jj] - beta*A[ii][jj]
Ainv[kk][jj]=Ainv[kk][jj] - beta*Ainv[ii][jj]
```
That's all there is to it. Just put it in a loop so that it iterates over all diagonal elements (ii index). Then eliminate each element in the column (kk index) except itself (if kk==ii: continue). The complete algorithm is in the code section.

Python Source Code mat_invert.py

```
import numpy as np

def mat_invert(A,epsilon=1E-10):

N = len(A)

Ainv=np.eye(N)

for ii in range(N):
alpha = A[ii][ii]
if abs(alpha) < epsilon: return (Ainv,-1)

for jj in range(N):
A[ii][jj] =  A[ii][jj]/alpha
Ainv[ii][jj] =  Ainv[ii][jj]/alpha

for kk in range(N):
if kk==ii: continue
beta = A[kk][ii]
for jj in range(N):
A[kk][jj]=A[kk][jj] - beta * A[ii][jj]
Ainv[kk][jj]=Ainv[kk][jj] - beta * Ainv[ii][jj]

return (Ainv,1)

if __name__ == "__main__":

from MUTIL import * # printMat and printVec utilities
import copy

C=np.array( [ [1.1,0.1,0.05],
[0.1,1.2,0.2 ],
[0.05,0.2,1.3] ])

svC=copy.deepcopy(C)

printMat(C,"%12.6f",'C mat')

invC,ok=mat_invert(C)
printMat(invC,"%12.6f",'C invert')
invCC=np.dot(invC,svC)
printMat(invCC,"%12.6f",'C invert times C')

**********Sample Output*********

C:\SCRIPTS\PYTHON\MATH>python mat_invert.py

C mat ( 3 , 3 )
1.100000    0.100000    0.050000
0.100000    1.200000    0.200000
0.050000    0.200000    1.300000

C invert ( 3 , 3 )
0.916767   -0.072376   -0.024125
-0.072376    0.860977   -0.129674
-0.024125   -0.129674    0.790109

C invert times C ( 3 , 3 )
1.000000    0.000000    0.000000
-0.000000    1.000000    0.000000
-0.000000    0.000000    1.000000
```

### Eigenvalues/Eigenvectors

Most compass calibrations algorithms invert a matrix to find the coefficients of the polynomial for an ellipse or ellipsoid. The solution is then shifted to the center, and an eigenvalue / eigenvector routine is applied in order to determine the full transformation matrix of the solution.

In some algorithms, the polynomials are also obtained using an eigenvalue / eigenvector routine. So, it is important to look at an eigenvalue routine.

A simple eigenvalue routine was derived by Kaiser (see references). I ran across it 40 years ago when looking for a method that was faster than the Givens-Householder method. It wasn't. But it is simpler. So, I cover it here.

I modified the routine slightly to address some of the problems encountered in solving for the polynomials using the direct least squares Fitzgibbon algorithm. The original routine was designed for matrices with positive eigenvalues. The Fitzgibbon method relies on a matrix that can have eigenvalues that are zero or negative. I found that the JK method worked for negative values, but since we have to divide by the eigenvalues to normalize the vectors, I eliminated solutions that were too close to zero. A passed parameter sets the zero threshold:

``` #set vectors to zero whose eigenvalues are too close to zero
absV=abs(values[jj])
if absV < Zthreshold:
for ii in range(N): vectors[ii][jj]=0
else:
for ii in range(N): vectors[ii][jj]=A[ii][jj]/values[jj]
```

If your matrices have all positive eigenvalues, then you can skip the step that returns the negative values. See the comments in the code.

The B matrix in the sample data has three eigenvalues that are too close to zero. See the output at the bottom of the listing. For comparison, I used the numpy library eig function. This matrix is an example of one that might be found using the direct least squares algorithm. Even though we get multiple eigenvalues / eigenvectors, only the eigenvector associated with the largest positive eigenvalue is used for the solution.

If all this seems too complicated, see Tom's Shortcut method that completely eliminates the need for an eigenvalue / eigenvector routine. Only a simple matrix inversion routine is required. (see above). This is ultra-handy in embedded systems where it might not be feasible to include a general eigenvector routine that can also solve for asymmetric matrices.

Python Source Code JK.py

```
# Compute the eigenvalues and eigenvectors for a symmetric matrix.
# Matrix is real. Eigenvectors are normalized with (divided by) the eigenvalue.
# For positive definite matricies this is not a problem.  Otherwise, compare the
# eigenvalues to a threshold, and if too close to zero, reject the solution.
# From:
# The JK Method: A procedure for finding the eigenvalues of a real
# symmetric matrix, H.F.Kaiser, The Computer Journal, Vol. 15 (1972), pp 271-273

import sys
import numpy as np
import copy

# Equation numbers are from original paper
# The JK method: a procedure for finding the eigenvectors
# and eigenvalues of a real symmetric matrix.
#   by H. F. Kaiser

def JK(A,Zthreshold=1e-6):

epsilon=1.0e-16
converge=1.0e-8
verbose=0
N=len(A)
values=np.zeros(N)
vectors=np.zeros((N,N))

lastDiag=0.0
svA=copy.deepcopy(A)
for iterLoop in range(15):

for jj in range(N-1):
for kk in range(jj+1,N):

num=0.0 #in the FORTRAN code, num is 'P' and den is 'Q'
den=0.0
for ii in range(N):
num = num +  A[ii][jj] * A[ii][kk]                             # numerator eq. 11
den = den + (A[ii][jj] + A[ii][kk]) *  (A[ii][jj] - A[ii][kk]) # denominator eq. 11
num = 2*num
anum=abs(num)
if anum < epsilon and den >=0:
break # halt k loop

tan2 = anum / aden                  #eq. 11
cos2 = 1 / np.sqrt(1 + tan2 * tan2) #eq. 12
sin2 = tan2 * cos2                  #eq. 13
else:
cot2 = aden / anum                  #eq. 16
sin2 = 1 / np.sqrt(1 + cot2 * cot2) #eq. 17
cos2 = cot2 * sin2                  #eq. 18

ca = np.sqrt((1 + cos2) / 2)           #eq. 14/19
sa = sin2 / (2 * ca)                   #eq. 15/20

if den < 0 :
tmp = ca
ca  = sa                            # table 21
sa  = tmp
if num < 0 :
sa= -sa

for ii in range(N):
tmp = A[ii][jj]
A[ii][jj] = tmp * ca + A[ii][kk] * sa
A[ii][kk] = -tmp * sa + A[ii][kk] * ca

da=np.diagonal(A)
if verbose==1:

if iterLoop > 5:
break # stop iterations if diagonal is not changing

for jj in range(N):
for kk in range(N):
values[jj]=values[jj] + A[kk][jj]* A[kk][jj]
values[jj] = np.sqrt(values[jj])   # these will all be positive, OK for positive definite matrix

#set vectors to zero whose eigenvalues are too close to zero
absV=abs(values[jj])

if absV < Zthreshold:
for ii in range(N): vectors[ii][jj]=0
else:
for ii in range(N): vectors[ii][jj]=A[ii][jj]/values[jj]

# get signed (negative) eigenvalues for non-positive definite matrix
# you can skip this step and return 'values' instead of 'svalu'
# if you are working with only positive eigenvalues

svalu=np.diagonal(np.dot(vectors.T,np.dot(svA,vectors)))

return (svalu,vectors)

if __name__ == "__main__":

from numpy.linalg import eig, inv

def printVec( vec, sfmt="%9.4F", title="vec" ):
ii = len(vec)
print '\n',title,'(',ii,')'
ss = " "
for ix in range(ii):
ari=np.real(vec[ix])
print ss
return
from numpy.linalg import eig, inv

def printMat( arr, sfmt="%9.4f", title="mat" ):
ii = len(arr)

jj = len(arr)
print '\n',title,'(',ii,',',jj,')'

for ix in range(ii):
ari=arr[ix]
ss = " "
for jx in range (jj):
arj=ari[jx]
print ss
return
zeroThreshold = 1e-6

B=np.array([[   0.000000,   0.000000,   0.055735,  -0.182271,  -1.362358,   1.544174],
[   0.000000,  -0.029843,   0.016174,  -0.047248,   2.517891,  -2.782243],
[   0.055735,   0.016174,  -0.112859,  -2.046633, -15.374392,  32.096392],
[  -0.182271,  -0.047248,  -2.046633,  14.592118, 108.775746,-171.281981],
[  -1.362358,   2.517891, -15.374392, 108.775746, 534.644957,-971.581651],
[   1.544174,  -2.782243,  32.096392,-171.281981,-971.581651,1515.448501]])

C=np.array( [ [1.0,0.1,0.05],
[0.1,1.0,0.2 ],
[0.05,0.2,1.0] ])

#in place modification of B and C matrices, so we need to keep a reference copy around

svC=copy.deepcopy(C)
svB=copy.deepcopy(B)

printMat(B,"%12.6f",'B mat')
# conventional eigenvalue/eigenvectors from numpy library
valB,vecB=eig(B)
vv=np.real(vecB)
print "\nSolution using numpy library"
printVec(valB,"%12.5f","B EigenValues")
printMat(vecB,"%12.6f",'B Eigenvectors')

# solution   using JK algorithm

(valBjk,vecBjk)=JK(B,zeroThreshold)
vecBjk=np.real(vecBjk)
printVec(valBjk,"%12.5f","JK B EigenValues")
printMat(vecBjk,"%12.6f",'JK B eigenvectors')

# do the same for a positive definite matrix

printMat(C,"%12.2f",'C mat')
valC,vecC=eig(C)
vv=np.real(vecC)
printVec(valC,"%12.5f","C numpy EigenValues")
printMat(vecC,"%12.6f",'C numpy Eigenvectors')

(valC,vecC)=JK(C,zeroThreshold)
printVec(valC,"%12.5f","C JK EigenValues")
printMat(vecC,"%12.6f",'C JK Eigenvectors')

*********SAMPLE OUTPUT**********

B mat ( 6 , 6 )
0.000000    0.000000    0.055735   -0.182271   -1.362358    1.544174
0.000000   -0.029843    0.016174   -0.047248    2.517891   -2.782243
0.055735    0.016174   -0.112859   -2.046633  -15.374392   32.096392
-0.182271   -0.047248   -2.046633   14.592118  108.775746 -171.281981
-1.362358    2.517891  -15.374392  108.775746  534.644957 -971.581651
1.544174   -2.782243   32.096392 -171.281981 -971.581651 1515.448501

Solution using numpy library

B EigenValues ( 6 )
2133.40657   -63.64643    -5.21727     0.00000    -0.00000    -0.00000

B Eigenvectors ( 6 , 6 )
-0.000955   -0.005524    0.000923   -0.474946   -0.708142    0.522426
0.001719    0.010975    0.073883    0.294578   -0.685592   -0.661517
-0.016602    0.058110   -0.305205    0.793816   -0.168265    0.494713
0.095320    0.047018    0.944175    0.231153    0.000111    0.209299
0.521741   -0.851007   -0.026389    0.048541   -0.012637    0.019002
-0.847597   -0.519662    0.096064    0.041459   -0.005063    0.023614

JK B EigenValues ( 6 )
2133.40657   -63.64643    -5.21727     0.00000     0.00000     0.00000

JK B eigenvectors ( 6 , 6 )
0.000955    0.005524    0.000923    0.000000    0.000000    0.000000
-0.001719   -0.010975    0.073883    0.000000    0.000000    0.000000
0.016602   -0.058110   -0.305205    0.000000    0.000000    0.000000
-0.095320   -0.047018    0.944175    0.000000    0.000000    0.000000
-0.521741    0.851007   -0.026389    0.000000    0.000000    0.000000
0.847597    0.519662    0.096064    0.000000    0.000000    0.000000

C mat ( 3 , 3 )
1.00        0.10        0.05
0.10        1.00        0.20
0.05        0.20        1.00

C numpy EigenValues ( 3 )
1.24622     0.96075     0.79303

C numpy Eigenvectors ( 3 , 3 )
0.399264    0.896252    0.193187
0.670284   -0.141577   -0.728474
0.625545   -0.420344    0.657270

C JK EigenValues ( 3 )
1.24622     0.96075     0.79303

C JK Eigenvectors ( 3 , 3 )
0.399264   -0.896252    0.193187
0.670284    0.141577   -0.728474
0.625545    0.420344    0.657270

```

### Robust Cholesky Decomposition

A Cholesky decomposition of a matrix creates a 'square root' lower triangular matrix which when multiplied by its transpose recovers the original matrix:

A = L*LT

Standard algorithms iterate through each row and form at some point a divisor in the form of a difference:

d = A[ii][ii] -S

We can't let this divisor go zero or negative. With noisy data it can. We can rescue the algorithm by forcing suspicious divisors to be a small positive number. The definition of 'small' depends on the data. In this algorithm I look at the ratio of the smallest to the largest divisor to get an idea of the scale of the problem. If it is too tiny, I make the substitution.

The justification for this approach is that we know there is a real solution to the problem. Our data may be marginal in just the wrong places to trigger a zero or negative divisor. So, we fix the data. This adds a small amount of noise. But for compass calibrations, I have found that the noise is insignificant by orders of magnitude relative to the accuracy of the compass.

Python Source Code choleskyx.py

```def choleskyX(A, ztol= 1.0e-5):
"""
Computes the lower triangular (LT) Cholesky factorization of
a positive definite matrix A.
"""
# Forward step for symmetric triangular L.
nrows = len(A)
errFlag=0
ztol2=ztol*ztol
L = np.zeros((nrows,nrows))
diag=np.diagonal(A)
absdmin=np.sqrt(np.dot(diag,diag))
absmax=1 # division by absmax below will always be defined
#   print "diag ",absdmin
for ii in range(nrows):
S=0.0
for k in range(ii):
S=S + L[k][ii]**2
d = A[ii][ii] -S
absd=abs(d)
if absd < absdmin:
absdmin=absd
if absd > absmax:
absmax = absd
absratio = absdmin/absmax
#      print "absmax absmin absratio",absmax,absdmin,absratio

if absratio < ztol2 or d<0:
L[ii][ii]=ztol*np.sqrt(absmax)
rat = absd/absmax
#         print "*** ",d," replaced by ",L[i][i]
print "*** Tolerance: ",ztol, " greater then ",np.sqrt(rat)
errFlag = -1
else:
L[ii][ii] = sqrt(d)

for j in range(ii+1, nrows):
S=0.0
for k in range(ii):
S = S + L[k][ii]*L[k][j]
#         if abs(S) < ztol:
#            S = 0.0
L[ii][j] = (A[ii][j] - S)/L[ii][ii]
L=L.T
return(L,errFlag,absdmin)

```

### Direct Least Squares Ellipse

#### Robust implementation of the Fitzgibbon, Pilu, and Fisher algorithm

The direct least squares method of Fitzgibbon, Pilu, and Fisher algorithm has two important features:

1. It is simple to implement. The original article solves everything with seven lines of MATLAB code
2. The constraint in the equations forces the solution to an ellipse (instead of another conic section). This is useful in situations where the data is noisy or marginal

The big drawback is that it is unstable with very low noise data. Consequently, Halir and Flusser developed a method of stabilization based on partitioning the matrix in question so that its eigenvalues were well behaved with noisy data.

Here, I develop a shorter and more pragmatic solution for compass calibrations. I split the matrix with a Cholesky decomposition. Unstable matrices will crash. I impose stability by forcing zero and small negative divisors in the Cholesky decomposition to be slightly positive. I have found that the errors introduced are smaller by multiple factors of ten compared to the compass calibration errors. Let's start from Fitzgibbon's eigenvalue equation:

S a = λ C a

a = λ S-1 C a [Eq. 1]

We want to find the eigenvalues and eigenvectors of S-1 C . There are two problems:

1. The matrix is unstable
2. The matrix is asymmetric
This last point is important because, while there are plenty of simple eigenvalue routines for symmetric matrices, it is difficult to come up with a general solution to asymmetric matrices. So let's try splitting S with the Cholesky decomposition.

S = L LT

We also substitute

a = ( L-1 )T b [Eq. 2]

and use

( A B )T = BT AT and ( A-1 )T = ( AT )-1

Plugging all of this into equation 1 above we arrive at

( L-1 )T b = λ( L-1 )T L-1 C ( L-1 )T b

or

b = λ( L-1 ) C ( L-1 )T b [Eq. 3]

or restating as a simple eigenvalue equation:

b = λ(symmetric matrix) b

So, we just use our simple SYMMETRIC matrix algorithm to get the eigenvectors b, and then use equation 2 to get the original eigenvectors a.

Sample of Robust implementation of Direct Least Squares Ellipse Algorithm Comparison of truth (red), Least Squares Ellipse (black) and Direct Least Squares with elliptical constraints (blue). The input data for this test is marginal. The iterative solution (magenta, not shown) failed to converge. The Least Squares is way off. The Direct Least Squares method performs best. Even so the compass error (5.87 degrees) would not be acceptable for calibrating a precision compass.

Python Source Code

The source code uses the three algorithms listed above:

1. choleskyX.py for the robust Cholesky decomposition of the S matrix
2. mat_invert.py to obtain the inverse of the Cholesky matrix
3. jk.py to obtain the eigenvalues and eigenvectors of a symmetric matrix

The source shows how to obtain the S and C matrices from the input data, and then picks up with the text to the left:

1. The robust Cholesky decomposition is performed on S
```      L,err,threshold=choleskyX(S,tolerance)
```
2. The inverse and transpose of L are calculated
```      (invL,status)=mat_invert(L)
invLT = invL.T
```
3. The symmetric matrix of equation 3 CC is formed and its eigenvectors determined:
```      CC=np.dot(invL,np.dot(C,invLT))
valC,vecC=JKOBI.JK(CC)
```
4. Finally, the eigenvectors for the original equation 1 are obtained via equation 2:
```      vvcl=np.dot(invLT,vecC)

```

```

def ls_ellipse_robust(xin,yin,tolerance=1e-5,verbose=0):
x = xin[:,np.newaxis]
y = yin[:,np.newaxis]
D = np.hstack((x*x, x*y, y*y, x, y, np.ones_like(x)))
S = np.dot(D.T,D)
C = np.zeros([6,6])
C[0,2] = C[2,0] = 2; C[1,1] = -1

if verbose> 0:
print "tolerance is ",tolerance
printMat(D,"%12.6f", "np Dmat" )
printMat(S,"%12.6f", "np Smat" )
printMat(C,"%12.6f", "np Cmat" )

try:
#tolerance is used to check for division by zero
L,err,threshold=choleskyX(S,tolerance)
if verbose > 0:
printMat(L,"%12.6f", "L" )

if err < 0:
print "Min L Diagonal ",threshold

(invL,status)=mat_invert(L)
if status < 0:
print "Invert L error, near zero diagonal"
invLT = invL.T
CC=np.dot(invL,np.dot(C,invLT))

if verbose > 0:
printMat(invL,"%12.6f", "inverse L" )
printMat(CC,"%12.6f", "lClT (CC matrix being eigened)" )

valC,vecC=JKOBI.JK(CC)
vvcl=np.dot(invLT,vecC)
vvcl=np.real(vvcl)

#get eigenVector from index of largest eValue

ix=np.argmax(valC) # JK sorts vectors so this should be zero
if verbose > 0: print 'Index is ',ix
cholv=vvcl.T[ix][:]
nrm=np.sqrt(np.dot(cholv,cholv))
cnrm=cholv/nrm

if verbose> 0:
printVec(valC,"%12.5f","tj EigenValues")
printMat(vvcl,"%12.5f","tj EigenVectors")
printVec(cnrm,"%12.5f", "Return normalized polynomial  : ")
except:
print "\nfit tj error"
return (-1,0,0,0)

return (1,cnrm,valC,vvcl)

```