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