Documentation Plus Sample Code

This is a very easy and very accurate algorithm for correcting hard iron errors in a magnetic compass. It uses all your data points to come up with a least-squares error fit. It is much better than the pencil and paper method that uses only four points (min and max on x and y coordinates) and throws all the rest away.

Least Squares Algorithms

- Circle: Quick, easy solution to simple 2D hard iron problem
- Sphere: Quick, easy solution to simple 3D hard iron problem
- Ellipse: 2D Hard and Soft iron solution
- Ellipsoid: 3D Hard and Soft iron solution
- Precision Ellipsoid: Precision Iterative 3D Hard and Soft iron solution

Algorithm

The classic equation for a circle centered at a,b with radius R is:

** ( x - a ) ^{2} + ( y - b )^{2} = R^{2} **

We can rearrange this to a form more suitable for a solution by rewriting it as:

**A (x ^{2} + y^{2}) + B x + C y = 1 **

Suppose now that we have n noisy input samples of each component x and y; n >= 3. More than the minimum
number of points are taken in order to prevent a small number of bad samples from seriously biasing the results.
Similarly, the samples are distributed around the circle to keep the solution from bulging out one
side or the other.

We can form the J matrix, an
n-row by 3-column matrix. The i^{th} row of this matrix is

** [(x _{i}^{2} + y_{i}^{2}) ,
x_{i} , y_{i} ] **

So, for example, if the i^{th} data point were at (3,2) then the i^{th} row
of the J matrix would be [(3^{2}+2^{2}),3,2] = [13,3,2].

Now collect the coefficients
A, B, and C into a column vector ABC, and we can write the n equations as

** J ABC = K **

where K is a column of ones. The J matrix is usually not square, so we cannot take its inverse. A modification of the equation makes it more inverse-friendly. Multiply both sides by the transpose of J. This creates a square matrix. We take the inverse of that matrix and multiply both sides by it. The resulting equation is

** ( J ^{T}J )^{-1} J^{T}J ABC =( J^{T}J )^{-1} J^{T} K **

** ABC =( J ^{T}J )^{-1} J^{T} K **

Everything on the right is known. So, we just form the matrices, take the indicated inverse, and multiply the matrices out. The output vector, ABC, has the components

** A = ABC[0]B = ABC[1]C = ABC[2]**

We convert back to standard form using

** a = xofs = -B/(2 A) b = yofs = -C/(2 A)R = sqrt(4 A + B**

Least Squares Fit to Circle using Noisy Samples

Python Source Code ls_circle.py

Inputs are two NumPy arrays for the observed x and y coordinates

Output is the tuple (X,Y,R) where X,Y is the center and R is the radius

```
```#least squares fit to a circle
import numpy as np
def ls_circle(xx,yy):
asize = np.size(xx)
print 'Circle input size is ' + str(asize)
J=np.zeros((asize,3))
ABC=np.zeros(asize)
K=np.zeros(asize)
for ix in range(0,asize):
x=xx[ix]
y=yy[ix]
J[ix,0]=x*x + y*y;
J[ix,1]=x;
J[ix,2]=y;
K[ix]=1.0
K=K.transpose()
JT=J.transpose()
JTJ = np.dot(JT,J)
InvJTJ=np.linalg.inv(JTJ);
ABC= np.dot(InvJTJ, np.dot(JT,K))
#If A is negative, R will be negative
A=ABC[0]
B=ABC[1]
C=ABC[2]
xofs=-B/(2*A)
yofs=-C/(2*A)
R=np.sqrt(4*A + B*B + C*C)/(2*A)
if R < 0.0: R = -R
return (xofs,yofs,R)
if __name__ == '__main__':
# Test of least squares fit to a circle
# Samples have random noise added to both X and Y components
# True center is at (1.5, 1.5); true radius is 1.2
# Output:
# Circle input size is 16
# Center at ( 1.5130, 1.5204) Radius: 1.2097
xx = np.array(
[ 2.2331, 2.5712, 2.7397, 2.6899,
2.4360, 2.0610, 1.5977, 1.1202,
0.7375, 0.3965, 0.2624, 0.4069,
0.6400, 1.0078, 1.4793, 1.9598])
yy = np.array (
[ 0.5676, 0.9002, 1.3492, 1.8300,
2.2661, 2.5607, 2.7243, 2.6918,
2.4032, 2.0740, 1.5964, 1.1327,
0.7084, 0.4101, 0.3221, 0.3840])
ans = ls_circle(xx,yy)
ss="Center at (%8.4f,%8.4f) Radius:%8.4f" % ans
print ss

MATLAB / Octave Source Code ls_circle.m

First two columns of input matrix are the observed x and y coordinates

Output is similar to the python code. Note that in Python an index starts at zero while MATLAB starts at 1

```
```function [xofs,yofs,R]=ls_circle(inmat)
[rr,cc]=size(inmat);
J=zeros(rr,3);
ABC=zeros(rr,3);
XX=inmat(:,1);
YY=inmat(:,2);
for iloop=1:rr
x=XX(iloop);
y=YY(iloop);
K(iloop)=1;
J(iloop,1)=x*x + y*y;
J(iloop,2)=x;
J(iloop,3)=y;
end
K=K';
JT=J';
JTJ = JT*J;
InvJTJ=inv(JTJ);
ABC= InvJTJ*JT*K;
A=ABC(1);
B=ABC(2);
C=ABC(3);
xofs=-B/(2*A);
yofs=-C/(2*A);
R=sqrt(4*A + B*B + C*C)/(2*A);
return;

©2017 Tom Judd

www.juddzone.com

Carlsbad, CA

USA