Aerospace Control Framework 1.0
|
Interactive Multiple Model Filter.
#include <sc_imm.h>
Public Member Functions | |
IMM () | |
Constructor. | |
~IMM () | |
Destructor. | |
void | intialize_filter_bank (const int size_bank) |
Sets the number of models in the filter bank. More... | |
void | set_filter_mode (const int n_filter, UKF_mode mode) |
Sets the filter mode. More... | |
void | set_filter_parameters (const int n_filter, const double alpha, const double kappa, const double beta) |
Sets the filter parameters. More... | |
void | set_integrator_time_info (double t, double dt) |
Sets the integrator time and time step. More... | |
void | set_initial_values (const ml_matrix &state0, const ml_matrix &covariance0, const ml_matrix ¶meter0=ml_matrix()) |
Sets the initial state and covariance. More... | |
void | set_noise_covariances (const int n_filter, const ml_matrix &Rp, const ml_matrix &Rm) |
Sets the noise covariance matrices. More... | |
void | set_state_function (const int n_filter, state_func state_fun) |
Sets the state function. More... | |
void | set_jump_function (jump_func jump_fun) |
Sets the jump function. More... | |
void | set_integrator (const int n_filter, integ_func integ_fun) |
Sets the integrator function. More... | |
void | set_measurement_function (const int n_filter, meas_func meas_fun) |
Sets the measurement function. More... | |
void | set_context (void *context) |
Sets the context pointer for data needed by the state or measurement functions. More... | |
ml_matrix | current_state (void) const |
Returns the current estimated state. | |
ml_matrix | current_covariance (void) const |
Returns the current estimated covariance. | |
ml_matrix | current_mode_probability (void) const |
Returns the current mode probability. | |
void | update_estimate (const ml_matrix &measurement) |
Updates the current estimate. More... | |
void IMM::intialize_filter_bank | ( | const int | bank_size | ) |
Initializes the bank of UKF filters.
bank_size | number of modes to be initialized. |
void IMM::set_filter_mode | ( | const int | n_filter, |
UKF_mode | mode | ||
) |
n_filter | filter to be updated |
mode | desired mode (from enum UKF_mode). |
void IMM::set_filter_parameters | ( | const int | n_filter, |
const double | alpha, | ||
const double | kappa, | ||
const double | beta | ||
) |
n_filter | filter to be updated |
alpha | usually in [.0001,1] |
kappa | 0 for state estimation |
beta | 2 for normally distributed data |
void IMM::set_integrator_time_info | ( | double | t, |
double | dt | ||
) |
t | time (s) |
dt | timestep (s) |
void IMM::set_initial_values | ( | const ml_matrix & | state0, |
const ml_matrix & | covariance0, | ||
const ml_matrix & | parameter0 = ml_matrix() |
||
) |
Sets the initial state, parameter and covariance.
n_filter | filter to be updated |
state0 | state (nx1) |
covariance0 | covariance (nxn) |
parameter0 | parameter (nx1) (not used in state mode) |
void IMM::set_noise_covariances | ( | const int | n_filter, |
const ml_matrix & | Rp, | ||
const ml_matrix & | Rm | ||
) |
n_filter | filter to be updated |
Rp | noise covariance matrix (nxn) |
Rm | measurement covariance matrix (mxm) |
void IMM::set_state_function | ( | const int | n_filter, |
state_func | state_fun | ||
) |
Sets the state function, which is the right hand side of the state update.
n_filter | filter to be updated |
state_fun | a function pointer to a state_func function |
void IMM::set_jump_function | ( | jump_func | jump_fun | ) |
jump_fun | a function pointer to a jump_func function |
void IMM::set_integrator | ( | const int | n_filter, |
integ_func | integ_fun | ||
) |
Sets the integrator function used to propagate the state.
n_filter | filter to be updated |
integ_fun | a function pointer to a integ_func function |
void IMM::set_measurement_function | ( | const int | n_filter, |
meas_func | meas_fun | ||
) |
n_filter | filter to be updated |
meas_fun | a function pointer to a meas_func function |
void IMM::set_context | ( | void * | context | ) |
Sets the context pointer that will be passed in to the other functions.
*context | a function pointer to a meas_func function |
void IMM::update_estimate | ( | const ml_matrix & | measurement | ) |
measurement | updated measurement vector (mx1) |