Implementing the Gauss-Newton Algorithm for Sphere Fitting (2 of 3)

I sketched a naïve implementation of the Gauss-Newton algorithm for sphere fitting, and provided an implementation in Octave. While it works fine on a laptop, it uses far too much memory to handle more than a handful of calibration samples on a microcontroller.

What took up all of that memory? The matrix , the vector , and the set of observations .

It turns out that we don’t really need to keep any of these around. In this post I’ll describe how to implement the algorithm without materializing and . This will reduce the memory footprint enough to make a practical algorithm that can handle ~100 observations. This is the algorithm used in the accelerometer calibration sketch I posted last year.

In the next post, I’ll show how to accumulate 25 statistics about the observations that have all of the information we need to run Gauss-Newton in constant space and time.

Let’s start by getting rid of the big arrays of statistics.

1. Only materialize what you need

When carrying out the Gauss-Newton algorithm as it was sketched in the last post, we never need direct access to the matrix entries of or . We just need to work with and — two arrays with sizes that do not depend on the number of samples collected.

We will materialize these matrices instead of the big matrices.

This is possible because each matrix entry in and can be written as a sum of functions of individual observations. This means we can look at an observation, compute the function of the observation, add it to the matrix entry, then move on to the next observation.

Let’s make this more concrete and look at the 00 entry of . Recall Equation 4 of part 1:

from which the definition of matrix multiplication gives

So to compute we can just accumulate this sum observation by observation. There is no need to compute the big matrices and .

The general matrix entries are as follows. For

for

and for

has a similar expression. Recall that

So for

and for

So all of the matrix entries we need to compute can be accumulated one observation at a time. We can store the observations, then at each iteration step in the Gauss-Newton algorithm run through these observations and use the formulas above to accumulate the matrix entries. Once we have done that, it is simple linear algebra to solve the matrix equation, find , and adjust . (Review the algorithm sketch here if needed.) This is essentially what is done in the sketch I posted last October, and I’ll use a slightly modified version of that code in the rest of this post.

2. Putting it into code

Now let’s translate this into C++. We won’t try to engineer the code here, just outline a simple Arduino sketch that does the job.

To start, we will need to declare some arrays to hold , and the observations . I’ll add an array for too even though it isn’t strictly needed.

//matrices for Gauss-Newton computations
float JtJ[6][6];
float JtR[6];
float delta[6];
float beta[6];

//Array of observations
int16_t* x;  //malloc this to length 3*N in setup().

This amounts to bytes, where is the number of samples. If we only stored the upper triangular part of , we could eliminate 15 floats, reducing the cost to bytes at the expense of making the code below a bit tougher to read. I will leave the modification as an exercise for the interested reader (but I’ll perform a similar one in the next post). The cost of the naïve algorithm was (had we declared arrays for and ), so this method uses significantly less memory for any number of samples. Importantly, the dependency on is dramatically reduced and this algorithm can now be used on and Arduino on data sets with ~100 samples.

With those declared, let’s look at the calibrate() routine. It starts by setting up some general parameters — how small a is small enough to stop? (eps), after how many iterations should we give up? (num_iterations), how big was the last ? (change) — then enters the main loop. There is nothing subtle about this loop, just

• clear the matrices
• compute the calibration matrices and
• find by solving the matrix equation
• measure the size of
• adjust

Here is what that looks like in code.

void calibrate() {
  int i;
  float eps = 0.000000001;
  int num_iterations = 20;
  float change = 100.0;
  while (--num_iterations >=0 && change > eps) {

    //set all of the matrix entries to zero.
    reset_calibration_matrices();

    //use the samples and the current values of beta to compute
    // the matrices JtJ and JtR
    compute_calibration_matrices();

    //solve the matrix equation JtJ*delta = JtR
    find_delta();
    
    //find the size of delta (look at log change in the coefficient parameters)
    change = delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2] + delta[3]*delta[3]/(beta[3]*beta[3]) + delta[4]*delta[4]/(beta[4]*beta[4]) + delta[5]*delta[5]/(beta[5]*beta[5]); 
    
    //adjust beta
    for(i=0;i<6;++i) {
      beta[i] -= delta[i];
    }
    
  }
    
}

But up to this is really just a template for an algorithm. As we’ve just been discussing, the real work of the algorithm all falls in to the routine compute_matrices(). Everything else is simple. In this implementation, we compute the matrices by scanning the samples and updating the matrix entries as required by Equations 1–5. Here is an implementation.

void compute_calibration_matrices() {
    int i;
  
    reset_calibration_matrices();
    for(i=0;i<N;i++) {    
      update_calibration_matrices(x+3*i);
    }
    
}


void update_calibration_matrices(const unsigned int* data) {
    int j, k;
    float dx, b;
    float residual = 1.0;
    float jacobian[6];
    
    for(j=0;j<3;++j) {
      b = beta[3+j];
      dx = ((float)data[j])- beta[j];
      residual -= pow(dx/b,2);
      jacobian[j] = 2.0*dx/pow(b,2);
      jacobian[3+j] = 2.0*dx*dx/pow(b,3);
    }
    
    for(j=0;j<6;++j) {
       JtR[j] += jacobian[j]*residual;
       for(k=0;k<6;++k) {
         JtJ[j][k] += jacobian[j]*jacobian[k];
       }
    }
    
}

3. Wrap-up and what’s next

By being careful about computing only the matrices we need, we were able to reduce the memory cost of the Gauss-Newton algorithm significantly and produce an algorithm that is viable on an inexpensive microcontroller. But we still need to keep the samples around and make a full pass through the sample set with each iterative pass — as changes so do the matrices. This costs space and time and limits the viability of the algorithm.

In the next post we will see that even here we are storing too much. Thanks to the nature of our residual function, we will see that there are 25 statistics that we can compute in one pass through the observation set that contain all of the information we need to compute and for any given . Once we do this, we will have an algorithm with space costs that are independent of the number of observations. (Well, really logarithmic because with too many samples a float will not have enough precision to capture the information in the marginal sample, but I’m not worrying about that for now.)