diff --git a/bbn_wave_freq_m5atomS3/KalmanFactoryFilter.h b/bbn_wave_freq_m5atomS3/KalmanFactoryFilter.h new file mode 100644 index 0000000..a80eecc --- /dev/null +++ b/bbn_wave_freq_m5atomS3/KalmanFactoryFilter.h @@ -0,0 +1,244 @@ +/*! +* \brief Initializes a named Kalman filter structure. +* +* This include requires the three defines {\ref KALMAN_NAME}, {\ref KALMAN_NUM_STATES} and +* {\ref KALMAN_NUM_INPUTS} to be set to the base name of the Kalman Filter and to the number +* of states and inputs respectively. +* +* It then will instantiate the buffers required for A, P, x as well as B, Q and u if the number +* of inputs is greater than zero, as well as the structure for the Kalman filter and the +* initialization method. +* +* Suppose the Kalman filter shall be named "acceleration", has three states and zero inputs. +* You would start by defining the required macros +* +* \code{.c} +* #define KALMAN_NAME acceleration +* #define KALMAN_NUM_STATES 3 +* #define KALMAN_NUM_INPUTS 0 +* \endcode +* +* After that, this file must be included +* +* \code{.c} +* #include "kalman_factory_filter_init.h" +* \endcode +* +* At this point, the structure \c kalman_filter_acceleration will be created (statically) along with +* all the required buffers (i.e. \c kalman_filter_acceleration_A_buffer, etc.) and the matrices +* will be initialized and set with the correct dimensions. +* +* In addition, a parameterless static initialization function \c {kalman_filter_acceleration_init()} will +* be created which you will need to call manually in order to set up the filter. +* +* To clean up the defined macros (e.g. in order to be able to create another named Kalman filter), +* you will have to include kalman_factory_cleanup.h: +* +* \code{.c} +* #include "kalman_factory_cleanup.h" +* \endcode +* +* A full example would be + +* \code{.c} +* #define KALMAN_NAME example +* #define KALMAN_NUM_STATES 4 +* #define KALMAN_NUM_INPUTS 0 + +* #include "kalman_factory_filter.h" +* // NOTE that this is the point to create measurement buffers +* #include "kalman_factory_cleanup.h" + +* void test_kalman() +* { +* kalman_filter_example_init(); +* kalman_filter_example.x.data[0] = 1; +* } +* \endcode +*/ + +/************************************************************************/ +/* Check for inputs */ +/************************************************************************/ + +#ifndef KALMAN_NAME +#error KALMAN_NAME needs to be defined prior to inclusion of this file. +#endif + +#ifndef KALMAN_NUM_STATES +#error KALMAN_NUM_STATES needs to be defined prior to inclusion of this file. +#elif KALMAN_NUM_STATES <= 0 +#error KALMAN_NUM_STATES must be a positive integer +#endif + +#ifndef KALMAN_NUM_INPUTS +#error KALMAN_NUM_INPUTS needs to be defined prior to inclusion of this file. +#elif KALMAN_NUM_INPUTS < 0 +#error KALMAN_NUM_INPUTS must be a positive integer or zero if no inputs are used +#endif + +/************************************************************************/ +/* Prepare dimensions */ +/************************************************************************/ + +#define __KALMAN_A_ROWS KALMAN_NUM_STATES +#define __KALMAN_A_COLS KALMAN_NUM_STATES + +#define __KALMAN_P_ROWS KALMAN_NUM_STATES +#define __KALMAN_P_COLS KALMAN_NUM_STATES + +#define __KALMAN_x_ROWS KALMAN_NUM_STATES +#define __KALMAN_x_COLS 1 + +#define __KALMAN_B_ROWS KALMAN_NUM_STATES +#define __KALMAN_B_COLS KALMAN_NUM_INPUTS + +#define __KALMAN_u_ROWS KALMAN_NUM_INPUTS +#define __KALMAN_u_COLS 1 + +#define __KALMAN_Q_ROWS KALMAN_NUM_STATES +#define __KALMAN_Q_COLS KALMAN_NUM_STATES + +#define __KALMAN_aux_ROWS ((KALMAN_NUM_STATES > KALMAN_NUM_INPUTS) ? KALMAN_NUM_STATES : KALMAN_NUM_INPUTS) +#define __KALMAN_aux_COLS 1 + +#define __KALMAN_tempP_ROWS __KALMAN_P_ROWS +#define __KALMAN_tempP_COLS __KALMAN_P_COLS + +#define __KALMAN_tempBQ_ROWS __KALMAN_B_ROWS +#define __KALMAN_tempBQ_COLS __KALMAN_B_COLS + +/************************************************************************/ +/* Name helper macro */ +/************************************************************************/ + +#ifndef STRINGIFY +#define __STRING2(x) #x +#define STRINGIFY(x) __STRING2(x) +#endif + +#pragma message("** Instantiating Kalman filter \"" STRINGIFY(KALMAN_NAME) "\" with " STRINGIFY(KALMAN_NUM_STATES) " states and " STRINGIFY(KALMAN_NUM_INPUTS) " inputs") + +#define __CONCAT(x, y) x ## y + +#define KALMAN_FILTER_BASENAME_HELPER(name) __CONCAT(kalman_filter_, name) +#define KALMAN_FILTER_BASENAME KALMAN_FILTER_BASENAME_HELPER(KALMAN_NAME) +#define KALMAN_BASENAME_HELPER(basename) __CONCAT(basename, _) + +/************************************************************************/ +/* Name macro */ +/************************************************************************/ + +#define KALMAN_BUFFER_HELPER2(basename, element) basename ## element ## _buffer +#define KALMAN_BUFFER_HELPER(basename, element) KALMAN_BUFFER_HELPER2(basename, element) +#define KALMAN_BUFFER_NAME(element) KALMAN_BUFFER_HELPER(KALMAN_BASENAME_HELPER(KALMAN_FILTER_BASENAME), element) + +#define KALMAN_FUNCTION_HELPER2(basename, name) basename ## name +#define KALMAN_FUNCTION_HELPER(basename, name) KALMAN_FUNCTION_HELPER2(basename, name) +#define KALMAN_FUNCTION_NAME(name) KALMAN_FUNCTION_HELPER(KALMAN_BASENAME_HELPER(KALMAN_FILTER_BASENAME), name) + +#define KALMAN_STRUCT_NAME KALMAN_FILTER_BASENAME + +/************************************************************************/ +/* Construct Kalman filter buffers: State */ +/************************************************************************/ + +#include "compiler.h" +#include "matrix.h" +#include "kalman.h" + +#define __KALMAN_BUFFER_A KALMAN_BUFFER_NAME(A) +#define __KALMAN_BUFFER_P KALMAN_BUFFER_NAME(P) +#define __KALMAN_BUFFER_x KALMAN_BUFFER_NAME(x) +#define __KALMAN_BUFFER_Q KALMAN_BUFFER_NAME(Q) + +#pragma message("Creating Kalman filter A buffer: " STRINGIFY(__KALMAN_BUFFER_A)) +static matrix_data_t __KALMAN_BUFFER_A[__KALMAN_A_ROWS * __KALMAN_A_COLS]; + +#pragma message("Creating Kalman filter P buffer: " STRINGIFY(__KALMAN_BUFFER_P)) +static matrix_data_t __KALMAN_BUFFER_P[__KALMAN_P_ROWS * __KALMAN_P_COLS]; + +#pragma message("Creating Kalman filter x buffer: " STRINGIFY(__KALMAN_BUFFER_x)) +static matrix_data_t __KALMAN_BUFFER_x[__KALMAN_x_ROWS * __KALMAN_x_COLS]; + +#pragma message("Creating Kalman filter Q buffer: " STRINGIFY(__KALMAN_BUFFER_Q)) +static matrix_data_t __KALMAN_BUFFER_Q[__KALMAN_Q_ROWS * __KALMAN_Q_COLS]; + +/************************************************************************/ +/* Construct Kalman filter buffers: Inputs */ +/************************************************************************/ + +#if KALMAN_NUM_INPUTS > 0 + +#define __KALMAN_BUFFER_B KALMAN_BUFFER_NAME(B) +#define __KALMAN_BUFFER_u KALMAN_BUFFER_NAME(u) + +#pragma message("Creating Kalman filter B buffer: " STRINGIFY(__KALMAN_BUFFER_B)) +static matrix_data_t __KALMAN_BUFFER_B[__KALMAN_B_ROWS * __KALMAN_B_COLS]; + +#pragma message("Creating Kalman filter u buffer: " STRINGIFY(__KALMAN_BUFFER_u)) +static matrix_data_t __KALMAN_BUFFER_u[__KALMAN_x_ROWS * __KALMAN_u_COLS]; + +#else + +#pragma message("Skipping Kalman filter B buffer: (zero inputs)") +#define __KALMAN_BUFFER_B ((matrix_data_t*)0) + +#pragma message("Skipping Kalman filter u buffer: (zero inputs)") +#define __KALMAN_BUFFER_u ((matrix_data_t*)0) + +#endif + +/************************************************************************/ +/* Construct Kalman filter buffers: Temporaries */ +/************************************************************************/ + +#define __KALMAN_BUFFER_aux KALMAN_BUFFER_NAME(aux) +#define __KALMAN_BUFFER_tempPBQ KALMAN_BUFFER_NAME(tempPBQ) + +#define __KALMAN_aux_size (__KALMAN_aux_ROWS * __KALMAN_aux_COLS) +#define __KALMAN_tempP_size (__KALMAN_tempP_ROWS * __KALMAN_tempP_COLS) +#define __KALMAN_tempBQ_size (__KALMAN_tempBQ_ROWS * __KALMAN_tempBQ_COLS) + +#define __KALMAN_tempPBQ_size ((__KALMAN_tempP_size > __KALMAN_tempBQ_size) ? __KALMAN_tempP_size : __KALMAN_tempBQ_size) + +#pragma message("Creating Kalman filter aux buffer: " STRINGIFY(__KALMAN_BUFFER_aux)) +static matrix_data_t __KALMAN_BUFFER_aux[__KALMAN_aux_size]; + +#pragma message("Creating Kalman filter temporary P/BQ buffer: " STRINGIFY(__KALMAN_BUFFER_tempPBQ)) +static matrix_data_t __KALMAN_BUFFER_tempPBQ[__KALMAN_tempPBQ_size]; + +/************************************************************************/ +/* Construct Kalman filter */ +/************************************************************************/ + +#pragma message("Creating Kalman filter structure: " STRINGIFY(KALMAN_STRUCT_NAME)) + +/*! +* \brief The Kalman filter structure +*/ +static kalman_t KALMAN_STRUCT_NAME; + +#pragma message ("Creating Kalman filter initialization function: " STRINGIFY(KALMAN_FUNCTION_NAME(init()) )) + +/*! +* \brief Initializes the Kalman Filter +* \return Pointer to the filter. +*/ +static kalman_t* KALMAN_FUNCTION_NAME(init)() { + int i; + for (i = 0; i < __KALMAN_x_ROWS * __KALMAN_x_COLS; ++i) { __KALMAN_BUFFER_x[i] = 0; } + for (i = 0; i < __KALMAN_A_ROWS * __KALMAN_A_COLS; ++i) { __KALMAN_BUFFER_A[i] = 0; } + for (i = 0; i < __KALMAN_P_ROWS * __KALMAN_P_COLS; ++i) { __KALMAN_BUFFER_P[i] = 0; } + for (i = 0; i < __KALMAN_Q_ROWS * __KALMAN_Q_COLS; ++i) { __KALMAN_BUFFER_Q[i] = 0; } + +#if KALMAN_NUM_INPUTS > 0 + for (i = 0; i < __KALMAN_B_ROWS * __KALMAN_B_COLS; ++i) { __KALMAN_BUFFER_B[i] = 0; } + for (i = 0; i < __KALMAN_u_ROWS * __KALMAN_x_COLS; ++i) { __KALMAN_BUFFER_u[i] = 0; } +#endif + + kalman_filter_initialize(&KALMAN_STRUCT_NAME, KALMAN_NUM_STATES, KALMAN_NUM_INPUTS, __KALMAN_BUFFER_A, __KALMAN_BUFFER_x, + __KALMAN_BUFFER_B, __KALMAN_BUFFER_u, __KALMAN_BUFFER_P, __KALMAN_BUFFER_Q, + __KALMAN_BUFFER_aux, __KALMAN_BUFFER_aux, __KALMAN_BUFFER_tempPBQ, __KALMAN_BUFFER_tempPBQ); + return &KALMAN_STRUCT_NAME; +}