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