Home | History | Annotate | Download | only in math 
      1 /*
      2  * This module contains the definition of a Levenberg-Marquardt solver for
      3  * non-linear least squares problems of the following form:
      4  *
      5  *   arg min  ||f(X)||  =  arg min  f(X)^T f(X)
      6  *      X                     X
      7  *
      8  * where X is an Nx1 state vector and f(X) is an Mx1 nonlinear measurement error
      9  * function of X which we wish to minimize the norm of.
     10  *
     11  * Levenberg-Marquardt solves the above problem through a damped Gauss-Newton
     12  * method where the solver step on each iteration is chosen such that:
     13  *       (J' J + uI) * step = - J' f(x)
     14  * where J = df(x)/dx is the Jacobian of the error function, u is a damping
     15  * factor, and I is the identity matrix.
     16  *
     17  * The algorithm implemented here follows Algorithm 3.16 outlined in this paper:
     18  * Madsen, Kaj, Hans Bruun Nielsen, and Ole Tingleff.
     19  * "Methods for non-linear least squares problems." (2004).
     20  *
     21  * This algorithm uses a variant of the Marquardt method for updating the
     22  * damping factor which ensures that changes in the factor have no
     23  * discontinuities or fluttering behavior between solver steps.
     24  */
     25 #ifndef LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_LEVENBERG_MARQUARDT_H_
     26 #define LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_LEVENBERG_MARQUARDT_H_
     27 
     28 #include <stddef.h>
     29 #include <stdint.h>
     30 
     31 #ifdef __cplusplus
     32 extern "C" {
     33 #endif
     34 
     35 // Function pointer for computing residual, f(X), and Jacobian, J = df(X)/dX
     36 // for a given state value, X, and other parameter data needed for computing
     37 // these terms, f_data.
     38 //
     39 // Note if f_data is not needed, it is allowable for f_data to be passed in
     40 // as NULL.
     41 //
     42 // jacobian is also allowed to be passed in as NULL, and in this case only the
     43 // residual will be computed and returned.
     44 typedef void (*ResidualAndJacobianFunction)(const float *state,
     45                                             const void *f_data,
     46                                             float *residual, float *jacobian);
     47 
     48 
     49 #define MAX_LM_STATE_DIMENSION (10)
     50 #define MAX_LM_MEAS_DIMENSION (20)
     51 
     52 // Structure containing fixed parameters for a single LM solve.
     53 struct LmParams {
     54   // maximum number of iterations allowed by the solver.
     55   uint32_t max_iterations;
     56 
     57   // initial scaling factor for setting the damping factor, i.e.:
     58   // damping_factor = initial_u_scale * max(J'J).
     59   float initial_u_scale;
     60 
     61   // magnitude of the cost function gradient required for solution, i.e.
     62   // max(gradient) = max(J'f(x)) < gradient_threshold.
     63   float gradient_threshold;
     64 
     65   // magnitude of relative error required for solution step, i.e.
     66   // ||step|| / ||state|| < relative_step_thresold.
     67   float relative_step_threshold;
     68 };
     69 
     70 // Structure containing data needed for a single LM solve.
     71 // Defining the data here allows flexibility in how the memory is allocated
     72 // for the solver.
     73 struct LmData {
     74   float step[MAX_LM_STATE_DIMENSION];
     75   float residual[MAX_LM_MEAS_DIMENSION];
     76   float residual_new[MAX_LM_MEAS_DIMENSION];
     77   float gradient[MAX_LM_MEAS_DIMENSION];
     78   float hessian[MAX_LM_STATE_DIMENSION * MAX_LM_STATE_DIMENSION];
     79   float temp[MAX_LM_STATE_DIMENSION * MAX_LM_MEAS_DIMENSION];
     80 };
     81 
     82 // Enumeration indicating status of the LM solver.
     83 enum LmStatus {
     84   RUNNING = 0,
     85   // Successful solve conditions:
     86   RELATIVE_STEP_SUFFICIENTLY_SMALL,  // solver step is sufficiently small.
     87   GRADIENT_SUFFICIENTLY_SMALL,  // cost function gradient is below threshold.
     88   HIT_MAX_ITERATIONS,
     89 
     90   // Solver failure modes:
     91   CHOLESKY_FAIL,
     92   INVALID_DATA_DIMENSIONS,
     93 };
     94 
     95 // Structure for containing variables needed for a Levenberg-Marquardt solver.
     96 struct LmSolver {
     97   // Solver parameters.
     98   struct LmParams params;
     99 
    100   // Pointer to solver data.
    101   struct LmData *data;
    102 
    103   // Function for computing residual (f(x)) and jacobian (df(x)/dx).
    104   ResidualAndJacobianFunction func;
    105 
    106   // Number of iterations in current solution.
    107   uint32_t num_iter;
    108 };
    109 
    110 // Initializes LM solver with provided parameters and error function.
    111 void lmSolverInit(struct LmSolver *solver, const struct LmParams *params,
    112                   ResidualAndJacobianFunction error_func);
    113 
    114 void lmSolverDestroy(struct LmSolver *solver);
    115 
    116 // Sets pointer for temporary data needed for an individual LM solve.
    117 // Note, this must be called prior to calling lmSolverSolve().
    118 void lmSolverSetData(struct LmSolver *solver, struct LmData *data);
    119 
    120 /*
    121  * Runs the LM solver for a given set of data and error function.
    122  *
    123  * INPUTS:
    124  * solver : pointer to an struct LmSolver structure.
    125  * initial_state : initial guess of the estimation state.
    126  * f_data : pointer to additional data needed by the error function.
    127  * state_dim : dimension of X.
    128  * meas_dim : dimension of f(X), defined in the error function.
    129  *
    130  * OUTPUTS:
    131  * LmStatus : enum indicating state of the solver on completion.
    132  * est_state : estimated state.
    133  */
    134 enum LmStatus lmSolverSolve(struct LmSolver *solver, const float *initial_state,
    135                             void *f_data, size_t state_dim, size_t meas_dim,
    136                             float *est_state);
    137 
    138 ////////////////////////// TEST UTILITIES ////////////////////////////////////
    139 // This function is exposed here for testing purposes only.
    140 /*
    141  * Computes the ratio of actual cost function gain over expected cost
    142  * function gain for the given solver step.  This ratio is used to adjust
    143  * the solver step size for the next solver iteration.
    144  *
    145  * INPUTS:
    146  * residual: f(x) for the current state x.
    147  * residual_new: f(x + step) for the new state after the solver step.
    148  * step: the solver step.
    149  * gradient: gradient of the cost function F(x).
    150  * damping_factor: the current damping factor used in the solver.
    151  * state_dim: dimension of the state, x.
    152  * meas_dim: dimension of f(x).
    153  */
    154 float computeGainRatio(const float *residual, const float *residual_new,
    155                        const float *step, const float *gradient,
    156                        float damping_factor, size_t state_dim,
    157                        size_t meas_dim);
    158 
    159 #ifdef __cplusplus
    160 }
    161 #endif
    162 
    163 #endif  // LOCATION_LBS_CONTEXTHUB_NANOAPPS_COMMON_MATH_LEVENBERG_MARQUARDT_H_
    164