├── .gitignore ├── AUTHORS ├── CMakeLists.txt ├── FUNCTIONS.rst ├── LICENSE ├── README.rst ├── compiler.h ├── conanfile.py ├── example_gravity.c ├── fixkalman.c ├── fixkalman.h ├── settings.h └── test_package ├── .gitignore ├── CMakeLists.txt ├── conanfile.py └── main.c /.gitignore: -------------------------------------------------------------------------------- 1 | ## Ignore Visual Studio temporary files, build results, and 2 | ## files generated by popular Visual Studio add-ons. 3 | 4 | # User-specific files 5 | *.suo 6 | *.user 7 | *.sln.docstates 8 | 9 | # Build results 10 | [Dd]ebug/ 11 | [Dd]ebugPublic/ 12 | [Rr]elease/ 13 | x64/ 14 | build/ 15 | bld/ 16 | [Bb]in/ 17 | [Oo]bj/ 18 | 19 | # MSTest test Results 20 | [Tt]est[Rr]esult*/ 21 | [Bb]uild[Ll]og.* 22 | 23 | #NUNIT 24 | *.VisualState.xml 25 | TestResult.xml 26 | 27 | *_i.c 28 | *_p.c 29 | *_i.h 30 | *.ilk 31 | *.meta 32 | *.obj 33 | *.pch 34 | *.pdb 35 | *.pgc 36 | *.pgd 37 | *.rsp 38 | *.sbr 39 | *.tlb 40 | *.tli 41 | *.tlh 42 | *.tmp 43 | *.tmp_proj 44 | *.log 45 | *.vspscc 46 | *.vssscc 47 | .builds 48 | *.pidb 49 | *.svclog 50 | *.scc 51 | 52 | # Chutzpah Test files 53 | _Chutzpah* 54 | 55 | # Visual C++ cache files 56 | ipch/ 57 | *.aps 58 | *.ncb 59 | *.opensdf 60 | *.sdf 61 | *.cachefile 62 | 63 | # Visual Studio profiler 64 | *.psess 65 | *.vsp 66 | *.vspx 67 | 68 | # TFS 2012 Local Workspace 69 | $tf/ 70 | 71 | # Guidance Automation Toolkit 72 | *.gpState 73 | 74 | # ReSharper is a .NET coding add-in 75 | _ReSharper*/ 76 | *.[Rr]e[Ss]harper 77 | *.DotSettings.user 78 | 79 | # JustCode is a .NET coding addin-in 80 | .JustCode 81 | 82 | # TeamCity is a build add-in 83 | _TeamCity* 84 | 85 | # DotCover is a Code Coverage Tool 86 | *.dotCover 87 | 88 | # NCrunch 89 | *.ncrunch* 90 | _NCrunch_* 91 | .*crunch*.local.xml 92 | 93 | # MightyMoose 94 | *.mm.* 95 | AutoTest.Net/ 96 | 97 | # Installshield output folder 98 | [Ee]xpress/ 99 | 100 | # DocProject is a documentation generator add-in 101 | DocProject/buildhelp/ 102 | DocProject/Help/*.HxT 103 | DocProject/Help/*.HxC 104 | DocProject/Help/*.hhc 105 | DocProject/Help/*.hhk 106 | DocProject/Help/*.hhp 107 | DocProject/Help/Html2 108 | DocProject/Help/html 109 | 110 | # Click-Once directory 111 | publish/ 112 | 113 | # Publish Web Output 114 | *.[Pp]ublish.xml 115 | *.azurePubxml 116 | 117 | # NuGet Packages Directory 118 | ## TODO: If you have NuGet Package Restore enabled, uncomment the next line 119 | #packages/* 120 | ## TODO: If the tool you use requires repositories.config, also uncomment the next line 121 | #!packages/repositories.config 122 | 123 | # Enable "build/" folder in the NuGet Packages folder since NuGet packages use it for MSBuild targets 124 | # This line needs to be after the ignore of the build folder (and the packages folder if the line above has been uncommented) 125 | !packages/build/ 126 | 127 | # Windows Azure Build Output 128 | csx/ 129 | *.build.csdef 130 | 131 | # Windows Store app package directory 132 | AppPackages/ 133 | 134 | # Others 135 | sql/ 136 | *.Cache 137 | ClientBin/ 138 | [Ss]tyle[Cc]op.* 139 | ~$* 140 | *~ 141 | *.dbmdl 142 | *.dbproj.schemaview 143 | *.pfx 144 | *.publishsettings 145 | node_modules/ 146 | 147 | # RIA/Silverlight projects 148 | Generated_Code/ 149 | 150 | # Backup & report files from converting an old project file to a newer 151 | # Visual Studio version. Backup files are not needed, because we have git ;-) 152 | _UpgradeReport_Files/ 153 | Backup*/ 154 | UpgradeLog*.XML 155 | UpgradeLog*.htm 156 | 157 | # SQL Server files 158 | App_Data/*.mdf 159 | App_Data/*.ldf 160 | 161 | # Business Intelligence projects 162 | *.rdl.data 163 | *.bim.layout 164 | *.bim_*.settings 165 | 166 | # Microsoft Fakes 167 | FakesAssemblies/ 168 | 169 | # GhostDoc 170 | *.GhostDoc.xml 171 | 172 | # ========================= 173 | # Windows detritus 174 | # ========================= 175 | 176 | # Windows image file caches 177 | Thumbs.db 178 | ehthumbs.db 179 | 180 | # Folder config file 181 | Desktop.ini 182 | 183 | # Recycle Bin used on file shares 184 | $RECYCLE.BIN/ 185 | 186 | 187 | .idea/ 188 | /__pycache__/ 189 | -------------------------------------------------------------------------------- /AUTHORS: -------------------------------------------------------------------------------- 1 | - Markus Mayer 2 | - David Bodnar -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.6) 2 | project(libfixkalman) 3 | 4 | include(${CMAKE_BINARY_DIR}/conanbuildinfo.cmake) 5 | conan_basic_setup() 6 | 7 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 8 | set(CMAKE_DEBUG_POSTFIX d) 9 | 10 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/bin) 11 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}) 12 | SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}) 13 | 14 | SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/lib) 15 | SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY_RELEASE ${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}) 16 | SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY_DEBUG ${CMAKE_ARCHIVE_OUTPUT_DIRECTORY}) 17 | 18 | set(SOURCE_FILES 19 | fixkalman.c) 20 | 21 | set(HEADER_FILES 22 | compiler.h 23 | settings.h 24 | fixkalman.h) 25 | 26 | set(GRAVITY_EXAMPLE_FILES 27 | example_gravity.c) 28 | 29 | add_library(libfixkalman STATIC ${HEADER_FILES} ${SOURCE_FILES}) 30 | add_executable(gravity_example ${HEADER_FILES} ${GRAVITY_EXAMPLE_FILES}) 31 | 32 | target_link_libraries(libfixkalman ${CONAN_LIBS}) 33 | target_link_libraries(gravity_example ${CONAN_LIBS} libfixkalman) -------------------------------------------------------------------------------- /FUNCTIONS.rst: -------------------------------------------------------------------------------- 1 | ================================ 2 | libfixkalman: Function reference 3 | ================================ 4 | 5 | .. contents :: 6 | 7 | Requirements 8 | ============ 9 | 10 | libfixkalman relies on `libfixmath `_ and `libfixmatrix `_ for all calculations. 11 | 12 | libfixmatrix requires ``FIXMATRIX_MAX_SIZE`` to be defined on the project to the largest matrix size. libfixkalman adds the constraint that ``FIXMATRIX_MAX_SIZE`` must be greater than or equal to 13 | the number of system states, inputs or observations (i.e. the largest of these values). If, for example, the system has 4 states, 1 input and 2 measured outputs (observations), ``FIXMATRIX_MAX_SIZE`` must be defined 14 | to at least 4. 15 | 16 | Preprocessor control 17 | ==================== 18 | 19 | In order to reduce code size, some features can be disabled by globally defining one ore multiple of the following: 20 | 21 | :KALMAN_DISABLE_UC: Disable filter functions for systems without control input. Set this if all your systems use control input or if you use certainty tuning. 22 | :KALMAN_DISABLE_C: Disable filter functions for systems with control inputs. Set this if none of your systems use control input. 23 | :KALMAN_DISABLE_LAMBDA: Disable filter functions with certainty tuning (lambda parameter). Set this if you do not need to control filter convergence speed. 24 | :KALMAN_JOSEPH_FORM: Enable the Joseph form of the covariance update equation. Set this if Kalman gain is non-optimal or your filter has problems with numerical stability. Please be aware that this form of the covariance update equation is computationally more expensive. Set this definition in settings.h. 25 | :KALMAN_TIME_VARYING: Enable time-varying Kalman filter for Kalman filter with control input. In this case the square system process noise matrix is replaced by the square contol input covariance matrix and the covariance prediction step changes from ``P = A*P*A' + Q`` to ``P = A*P*A' + B*Q*B'``. Set this definition in settings.h. 26 | 27 | Definitions 28 | =========== 29 | 30 | There are two kinds of filters that can be processed with this library: Regular filters described by state transition and control input and such systems without control inputs (dubbed 'uncontrolled'). 31 | For each of these types a separate set of functions exists: Regular functions and functions postfixed with ``_uc`` (such as `kalman_predict_uc`_) for uncontrolled systems. 32 | 33 | kalman16_t 34 | ---------- 35 | Data structure for the filter state. :: 36 | 37 | typedef struct { 38 | mf16 x; // S x 1 39 | mf16 A; // S x S 40 | mf16 P; // S x S 41 | mf16 u; // C x 1 42 | mf16 B; // S x C 43 | mf16 Q; // C x C if KALMAN_TIME_VARYING is defined 44 | mf16 Q; // S x S if KALMAN_TIME_VARYING is not defined 45 | } kalman16_t; 46 | 47 | :x: System state vector. Number of rows in the vector, 1 <= rows <= FIXMATRIX_MAX_SIZE. 48 | :A: Square system state transition model matrix. Number of rows and columns in the matrix is equal to the number of rows in the state vector ``x``. 49 | :P: Square system state covariance matrix. Number of rows and columns is identical to ``A``. 50 | :u: Input vector. Number of rows in the vector, 1 <= rows <= FIXMATRIX_MAX_SIZE. 51 | :B: Control input model matrix. Number of rows in the matrix is equal to the number of rows in the state vector ``x``. Number of columns in the matrix is equal to the number of rows in the input vector ``u``. 52 | :Q: Square contol input covariance matrix. Number of rows and columns in the matrix is equal to the number of rows in the input vector ``u``. (if KALMAN_TIME_VARYING is defined) 53 | :Q: Square system process noise matrix. Number of rows and columns is identical to ``A``. (if KALMAN_TIME_VARYING is not defined) 54 | 55 | The filter structure can be initialized by calling 56 | 57 | ``kalman_filter_initialize(&filter, NUM_STATES, NUM_INPUTS);`` 58 | 59 | kalman16_uc_t 60 | ------------- 61 | Data structure for the uncontrolled (inputless) filter state. :: 62 | 63 | typedef struct { 64 | mf16 x; // S x 1 65 | mf16 A; // S x S 66 | mf16 P; // S x S 67 | mf16 Q; // S x S 68 | } kalman16_t; 69 | 70 | :x: System state vector. Number of rows in the vector, 1 <= rows <= FIXMATRIX_MAX_SIZE. 71 | :A: Square system state transition model matrix. Number of rows and columns in the matrix is equal to the number of rows in the state vector ``x``. 72 | :P: Square system state covariance matrix. Number of rows and columns is identical to ``A``. 73 | :Q: Square system process noise matrix. Number of rows and columns is identical to ``A``. 74 | 75 | The filter structure can be initialized by calling 76 | 77 | ``kalman_filter_initialize_uc(&filter, NUM_STATES);`` 78 | 79 | kalman16_observation_t 80 | ---------------------- 81 | Data structure for the measurement updates. :: 82 | 83 | typedef struct { 84 | mf16 z; // Z x 1 85 | mf16 H; // Z x S 86 | mf16 R; // Z x Z 87 | } kalman16_t; 88 | 89 | :z: Observation vector. Number of rows in the vector, 1 <= rows <= FIXMATRIX_MAX_SIZE. 90 | :H: Observation model matrix. Number of rows in the matrix is equal to the number of rows in the measurement vector ``z``. Number of columns in the matrix is equal to the number of rows in the state vector ``x``. 91 | :R: Square observation covariance matrix. Number of rows and columns is identical to the number of rows in the measurement vector ``z``. 92 | 93 | The filter structure can be initialized by calling 94 | 95 | ``kalman_observation_initialize(&filter, NUM_STATES, NUM_OBSERVATIONS);`` 96 | 97 | Initialization Functions 98 | ======================== 99 | 100 | kalman_filter_initialize 101 | ------------------------ 102 | Initializes a *kalman16_t* structure:: 103 | 104 | void kalman_filter_initialize(kalman16_t *const kf, uint_fast8_t num_states, uint_fast8_t num_inputs); 105 | 106 | :kf: The filter structure to initialize. 107 | :num_states: The number of system states. 108 | :num_inputs: The number of system inputs. 109 | 110 | kalman_filter_initialize_uc 111 | ---------------------------- 112 | Initializes a *kalman16_uc_t* structure:: 113 | 114 | void kalman_filter_initialize_uc(kalman16_uc_t *const kf, uint_fast8_t num_states); 115 | 116 | :kf: The filter structure to initialize. 117 | :num_states: The number of system states. 118 | 119 | kalman_observation_initialize 120 | ----------------------------- 121 | Initializes a *kalman16_observation_t* structure:: 122 | 123 | void kalman_observation_initialize(kalman16_observation_t *const kfm, uint_fast8_t num_states, uint_fast8_t num_observations); 124 | 125 | :kf: The observation structure to initialize. 126 | :num_states: The number of system states. 127 | :num_observations: The number of observations. 128 | 129 | Prediction and Update Functions (regular systems) 130 | ================================================= 131 | 132 | kalman_predict 133 | -------------- 134 | Kalman filter prediction (time update) step:: 135 | 136 | void kalman_predict(kalman16_t *kf); 137 | 138 | :kf: The filter to update. 139 | 140 | This performs a state and covariance update according to the state transition model *A* and the input model *B*. If *B* has zero dimensions, only the state transition model will be used. 141 | 142 | This function is a thin wrapper around `kalman_predict_x`_ and `kalman_predict_P`_. 143 | It is often more efficient to perform the state update manually instead of relying on the matrix multiplication algorithm. In this case, `kalman_predict_P`_ can be used to update the system covariance 144 | matrix afterwards. 145 | 146 | If input values are used, the user is required to set the values in ``kfm.u`` prior to calling this function. 147 | 148 | kalman_predict_tuned 149 | -------------------- 150 | Kalman filter prediction (time update) step with applied certainty tuning:: 151 | 152 | void kalman_predict_tuned(kalman16_t *kf, fix16_t lambda); 153 | 154 | :kf: The filter to update. 155 | :lambda: The estimation certainty tuning factor. 0.0 < lambda <= 1.0; 156 | 157 | This performs a state and covariance update according to the state transition model *A* and the input model *B*. If *B* has zero dimensions, only the state transition model will be used. 158 | In addition, the system covariance matrix will be scaled by the factor 1/lambda^2. This can be used to artificially increase prediction uncertainty to prevent convergence. 159 | 160 | If input values are used, the user is required to set the values in ``kfm.u`` prior to calling this function. 161 | 162 | Similar to *kalman_predict()*, this function is a thin wrapper around `kalman_predict_x`_ and `kalman_predict_P_tuned`_. 163 | It is often more efficient to perform the state update manually instead of relying on the matrix multiplication algorithm. In this case, `kalman_predict_P_tuned`_ can be used to update the system covariance 164 | matrix afterwards. 165 | 166 | kalman_predict_x 167 | ---------------- 168 | Kalman filter state-only prediction (time update) step:: 169 | 170 | void kalman_predict_x(kalman16_t *kf); 171 | 172 | :kf: The filter to update. 173 | 174 | This performs a state-only (i.e. no covariance) update according to the state transition model *A* and the input model *B*. If *B* has zero dimensions, only the state transition model will be used. 175 | 176 | If input values are used, the user is required to set the values in ``kfm.u`` prior to calling this function. 177 | 178 | kalman_predict_P 179 | ---------------- 180 | Kalman filter covariance-only prediction (time update) step:: 181 | 182 | void kalman_predict_P(kalman16_t *kf); 183 | 184 | :kf: The filter to update. 185 | 186 | This performs a covariance-only (i.e. no state) update according to the state transition model *A* and the input model *B*. If *B* has zero dimensions, only the state transition model will be used. 187 | 188 | In cases where it is more efficient to calculate the state update manually (i.e. by not calling `kalman_predict`_), *kalman_predict_P* can be used to update the covariance matrix. 189 | 190 | kalman_predict_P_tuned 191 | ---------------------- 192 | Kalman filter covariance-only prediction (time update) step with certainty tuning:: 193 | 194 | void kalman_predict_P_tuned(kalman16_t *kf, fix16_t lambda); 195 | 196 | :kf: The filter to update. 197 | :lambda: The estimation certainty tuning factor. 0.0 < lambda <= 1.0 198 | 199 | Similar to ``kalman_predict_P()``, this function performs a covariance-only (i.e. no state) update according to the state transition model *A* and the input model *B*. If *B* has zero dimensions, only the state transition model will be used. 200 | In addition, the system covariance matrix will be scaled by the factor 1/lambda^2. This can be used to artificially increase prediction uncertainty to prevent convergence. 201 | 202 | In cases where it is more efficient to calculate the state update manually (i.e. by not calling `kalman_predict_tuned`_), *kalman_predict_P_tuned* can be used to update the covariance matrix. 203 | 204 | kalman_correct 205 | -------------- 206 | Kalman filter correction (measurement update) step:: 207 | 208 | void kalman_correct(kalman16_t *kf, kalman16_observation_t *kfm); 209 | 210 | :kf: The filter to update. 211 | :kfm: The observation used to update the filter. 212 | 213 | This updates the state estimation as retrieved from the prediction functions and corrects the estimate using the observation in *kfm*. 214 | 215 | The user is required to set the values in ``kfm.z`` (and ``kfm.R`` if required) prior to calling this function. 216 | 217 | Prediction and Update Functions (systems without control input) 218 | =============================================================== 219 | 220 | kalman_predict_uc 221 | ------------------ 222 | Kalman filter prediction (time update) step:: 223 | 224 | void kalman_predict_uc(kalman16_uc_t *kf); 225 | 226 | :kf: The filter to update. 227 | 228 | This performs a state and covariance update according to the state transition model *A*. 229 | 230 | This function is a thin wrapper around `kalman_predict_x_uc`_ and `kalman_predict_P_uc`_. 231 | It is often more efficient to perform the state update manually instead of relying on the matrix multiplication algorithm. In this case, `kalman_predict_P_uc`_ can be used to update the system covariance 232 | matrix afterwards. 233 | 234 | If input values are used, the user is required to set the values in ``kfm.u`` prior to calling this function. 235 | 236 | kalman_cpredict_uc 237 | ------------------ 238 | Kalman filter continuous-time prediction (time update) and integration step:: 239 | 240 | void kalman_predict_uc(kalman16_uc_t *kf, register fix16_t deltaT); 241 | 242 | :kf: The filter to update. 243 | :deltaT: The time differential in seconds. 244 | 245 | This performs a state and covariance update according to the state transition model *A*. 246 | 247 | This function is a thin wrapper around `kalman_cpredict_x_uc`_ and `kalman_cpredict_P_uc`_. 248 | It is often more efficient to perform the state update manually instead of relying on the matrix multiplication algorithm. In this case, `kalman_cpredict_P_uc`_ can be used to update the system covariance 249 | matrix afterwards. 250 | 251 | If input values are used, the user is required to set the values in ``kfm.u`` prior to calling this function. 252 | 253 | kalman_predict_tuned_uc 254 | ----------------------- 255 | Kalman filter prediction (time update) step with applied certainty tuning:: 256 | 257 | void kalman_predict_tuned_uc(kalman16_uc_t *kf, fix16_t lambda); 258 | 259 | :kf: The filter to update. 260 | :lambda: The estimation certainty tuning factor. 0.0 < lambda <= 1.0; 261 | 262 | This performs a state and covariance update according to the state transition model *A*. 263 | In addition, the system covariance matrix will be scaled by the factor 1/lambda^2. This can be used to artificially increase prediction uncertainty to prevent convergence. 264 | 265 | If input values are used, the user is required to set the values in ``kfm.u`` prior to calling this function. 266 | 267 | Similar to *kalman_predict_uc()*, this function is a thin wrapper around `kalman_predict_x_uc`_ and `kalman_predict_P_tuned_uc`_. 268 | It is often more efficient to perform the state update manually instead of relying on the matrix multiplication algorithm. In this case, `kalman_predict_P_tuned_uc`_ can be used to update the system covariance 269 | matrix afterwards. 270 | 271 | kalman_predict_x_uc 272 | ------------------- 273 | Kalman filter state-only prediction (time update) step:: 274 | 275 | void kalman_predict_x_uc(kalman16_uc_t *kf); 276 | 277 | :kf: The filter to update. 278 | 279 | This performs a state-only (i.e. no covariance) update according to the state transition model *A*. 280 | 281 | If input values are used, the user is required to set the values in ``kfm.u`` prior to calling this function. 282 | 283 | kalman_cpredict_x_uc 284 | ------------------- 285 | Kalman filter continuous-time state-only prediction (time update) and integration step:: 286 | 287 | void kalman_predict_x_uc(kalman16_uc_t *kf, register fix16_t deltaT); 288 | 289 | :kf: The filter to update. 290 | :deltaT: The time differential in seconds, 291 | 292 | This performs a state-only (i.e. no covariance) update according to the state transition model *A*. 293 | 294 | If input values are used, the user is required to set the values in ``kfm.u`` prior to calling this function. 295 | 296 | kalman_predict_P_uc 297 | ------------------- 298 | Kalman filter covariance-only prediction (time update) step:: 299 | 300 | void kalman_predict_P_uc(kalman16_uc_t *kf); 301 | 302 | :kf: The filter to update. 303 | 304 | This performs a covariance-only (i.e. no state) update according to the state transition model *A*. 305 | 306 | In cases where it is more efficient to calculate the state update manually (i.e. by not calling `kalman_predict`_), *kalman_predict_P* can be used to update the covariance matrix. 307 | 308 | kalman_cpredict_P_uc 309 | ------------------- 310 | Kalman filter cintinuous-time covariance-only prediction (time update) and integration step:: 311 | 312 | void kalman_predict_P_uc(kalman16_uc_t *kf, register fix16_t deltaT); 313 | 314 | :kf: The filter to update. 315 | :deltaT: The time differential. 316 | 317 | This performs a covariance-only (i.e. no state) update according to the state transition model *A*. 318 | 319 | In cases where it is more efficient to calculate the state update manually (i.e. by not calling `kalman_cpredict`_), *kalman_cpredict_P* can be used to update the covariance matrix. 320 | 321 | kalman_predict_P_tuned_uc 322 | -------------------------- 323 | Kalman filter covariance-only prediction (time update) step with certainty tuning:: 324 | 325 | void kalman_predict_P_tuned_uc(kalman16_uc_t *kf, fix16_t lambda); 326 | 327 | :kf: The filter to update. 328 | :lambda: The estimation certainty tuning factor. 0.0 < lambda <= 1.0 329 | 330 | Similar to ``kalman_predict_P()``, this function performs a covariance-only (i.e. no state) update according to the state transition model *A*. 331 | In addition, the system covariance matrix will be scaled by the factor 1/lambda^2. This can be used to artificially increase prediction uncertainty to prevent convergence. 332 | 333 | In cases where it is more efficient to calculate the state update manually (i.e. by not calling `kalman_predict_tuned_uc`_), *kalman_predict_P_tuned_uc* can be used to update the covariance matrix. 334 | 335 | kalman_correct_uc 336 | ----------------- 337 | Kalman filter correction (measurement update) step:: 338 | 339 | void kalman_correct_uc(kalman16_uc_t *kf, kalman16_observation_t *kfm); 340 | 341 | :kf: The filter to update. 342 | :kfm: The observation used to update the filter. 343 | 344 | This updates the state estimation as retrieved from the prediction functions and corrects the estimate using the observation in *kfm*. 345 | 346 | The user is required to set the values in ``kfm.z`` (and ``kfm.R`` if required) prior to calling this function. 347 | 348 | 349 | Helper Functions (regular systems) 350 | ================================== 351 | 352 | kalman_get_state_vector 353 | ----------------------- 354 | Retrieves a pointer to the state vector *x*:: 355 | 356 | mf16* kalman_get_state_vector(kalman16_t *kf); 357 | 358 | :kf: The filter. 359 | 360 | kalman_get_state_transition 361 | --------------------------- 362 | Retrieves a pointer to the state transition model *A*:: 363 | 364 | mf16* kalman_get_state_transition(kalman16_t *kf); 365 | 366 | :kf: The filter. 367 | 368 | kalman_get_system_covariance 369 | ---------------------------- 370 | Retrieves a pointer to the system covariance matrix *P*:: 371 | 372 | mf16* kalman_get_system_covariance(kalman16_t *kf); 373 | 374 | :kf: The filter. 375 | 376 | kalman_get_input_vector 377 | ----------------------- 378 | Retrieves a pointer to the control input vector *u*:: 379 | 380 | mf16* kalman_get_input_vector(kalman16_t *kf); 381 | 382 | :kf: The filter. 383 | 384 | kalman_get_input_transition 385 | --------------------------- 386 | Retrieves a pointer to the control input transition model *B*:: 387 | 388 | mf16* kalman_get_input_transition(kalman16_t *kf) 389 | 390 | :kf: The filter. 391 | 392 | kalman_get_input_covariance 393 | --------------------------- 394 | Retrieves a pointer to the control input covariance matrix *Q*:: 395 | 396 | mf16* kalman_get_input_covariance(kalman16_t *kf) 397 | 398 | :kf: The filter. 399 | 400 | Helper Functions (systems without control input) 401 | ================================================ 402 | 403 | kalman_get_state_vector_uc 404 | -------------------------- 405 | Retrieves a pointer to the state vector *x*:: 406 | 407 | mf16* kalman_get_state_vector_uc(kalman16_uc_t *kf); 408 | 409 | :kf: The filter. 410 | 411 | kalman_get_state_transition_uc 412 | ------------------------------ 413 | Retrieves a pointer to the state transition model *A*:: 414 | 415 | mf16* kalman_get_state_transition_uc(kalman16_uc_t *kf); 416 | 417 | :kf: The filter. 418 | 419 | kalman_get_system_covariance_uc 420 | -------------------------------- 421 | Retrieves a pointer to the system covariance matrix *P*:: 422 | 423 | mf16* kalman_get_system_covariance_uc(kalman16_uc_t *kf); 424 | 425 | :kf: The filter. 426 | 427 | kalman_get_system_process_noise_uc 428 | ---------------------------------- 429 | Retrieves a pointer to the system process noise matrix *Q*:: 430 | 431 | mf16* kalman_get_system_process_noise_uc(kalman16_t *kf) 432 | 433 | :kf: The filter. 434 | 435 | Helper Functions (observations) 436 | =============================== 437 | 438 | kalman_get_observation_vector 439 | ----------------------------- 440 | Retrieves a pointer to the observation vector *z*:: 441 | 442 | mf16* kalman_get_observation_transformation(kalman16_observation_t *kfm) 443 | 444 | :kfm: The measurement. 445 | 446 | kalman_get_observation_process_noise 447 | ------------------------------------ 448 | Retrieves a pointer to the process noise matrix *R*:: 449 | 450 | mf16* kalman_get_observation_process_noise(kalman16_observation_t *kfm) 451 | 452 | :kfm: The measurement. 453 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 Markus Mayer 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining 6 | a copy of this software and associated documentation files (the 7 | "Software"), to deal in the Software without restriction, including 8 | without limitation the rights to use, copy, modify, merge, publish, 9 | distribute, sublicense, and/or sell copies of the Software, and to 10 | permit persons to whom the Software is furnished to do so, subject to 11 | the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be 14 | included in all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 20 | BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 21 | ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 22 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. -------------------------------------------------------------------------------- /README.rst: -------------------------------------------------------------------------------- 1 | .. image:: https://raw.githubusercontent.com/sunsided/libfixkalman/static/kalman.png 2 | :align: right 3 | 4 | Fixed point Kalman filter library 5 | ================================= 6 | 7 | libfixkalman is a Kalman filter computation library for microcontrollers. 8 | It is based on the libfixmatrix_ and libfixmath_ libraries, which use 16.16 bit fixed point values. 9 | The main focus is processors without an FPU, such as ARM Cortex-M0 or M3. 10 | 11 | A 🦀 Rust variant, albeit not a direct port, is available at `sunsided/minikalman-rs`_. 12 | 13 | ---- 14 | 15 | Matrix inversion in the correction step is implemented using Cholesky decomposition and an optimized 16 | inversion algorithm ported from EJML_. 17 | 18 | See `function reference`_ for further details and `example_gravity.c`_ for example code. 19 | 20 | 21 | .. _libfixmath: http://code.google.com/p/libfixmath/ 22 | .. _libfixmatrix: https://github.com/PetteriAimonen/libfixmatrix 23 | .. _EJML: https://code.google.com/p/efficient-java-matrix-library/ 24 | .. _function reference: https://github.com/sunsided/libfixkalman/blob/master/FUNCTIONS.rst 25 | .. _`example_gravity.c`: https://github.com/sunsided/libfixkalman/blob/master/example_gravity.c 26 | .. `sunsided/minikalman-rs`: https://github.com/sunsided/minikalman-rs 27 | 28 | conan.io 29 | -------- 30 | 31 | This library now has experimental support for the `conan.io`_ package manager and is aimed at CMake. Both ``libfixmath`` and ``libfixmatrix`` dependencies are available on conan.io and you should be able to verify the package building process by calling:: 32 | 33 | conan test_package --build 34 | 35 | In general, to reference the library you'd provide a ``conanfile.txt`` with the following content:: 36 | 37 | [requires] 38 | libfixkalman/20161008@sunside/stable 39 | 40 | which corresponds to `this package`_, where ``20161008`` could be replaced with the latest version as found via ``conan search -v libfixkalman* -r=conan.io``. You can then just:: 41 | 42 | conan install 43 | 44 | or:: 45 | 46 | conan install --build 47 | 48 | to obtain all required references. 49 | 50 | .. _`conan.io`: https://conan.io/ 51 | .. _`this package`: https://conan.io/source/libfixkalman/20161008/sunside/stable 52 | -------------------------------------------------------------------------------- /compiler.h: -------------------------------------------------------------------------------- 1 | #ifndef _COMPILER_H_ 2 | #define _COMPILER_H_ 3 | 4 | /** 5 | * \def RESTRICT Marks a restricted pointer 6 | */ 7 | #ifdef __GNUC__ 8 | #define RESTRICT __restrict__ 9 | #else 10 | #define RESTRICT 11 | #endif 12 | 13 | /** 14 | * \def PURE Marks a function as pure, i.e. without global state 15 | */ 16 | #ifdef __GNUC__ 17 | #define PURE __attribute__ ((pure)) 18 | #else 19 | #define PURE 20 | #endif 21 | 22 | /** 23 | * \def CONST Marks a function as const, i.e. without global state and wihout using external memory 24 | */ 25 | #ifdef __GNUC__ 26 | #define CONST __attribute__ ((const)) 27 | #else 28 | #define CONST 29 | #endif 30 | 31 | /** 32 | * \def HOT Marks a function as a hot spot 33 | */ 34 | #ifdef __GNUC__ 35 | #define HOT __attribute__ ((hot)) 36 | #else 37 | #define HOT 38 | #endif 39 | 40 | /** 41 | * \def COLD Marks a function as a cold spot 42 | */ 43 | #ifdef __GNUC__ 44 | #define COLD __attribute__ ((cold)) 45 | #else 46 | #define COLD 47 | #endif 48 | 49 | /** 50 | * \def LEAF Marks a function as leaf 51 | */ 52 | #ifdef __GNUC__ 53 | #define LEAF __attribute__ ((leaf)) 54 | #else 55 | #define LEAF 56 | #endif 57 | 58 | /** 59 | * \def NONNULL Marks all function parameters as non-null 60 | */ 61 | #ifdef __GNUC__ 62 | #define NONNULL __attribute__ ((nonnull)) 63 | #else 64 | #define NONNULL 65 | #endif 66 | 67 | /** 68 | * \def RETURNS_NONNULL Marks the function return value as being non-null 69 | */ 70 | #ifdef __GNUC__ 71 | #define RETURNS_NONNULL __attribute__ ((returns_nonnull)) 72 | #else 73 | #define RETURNS_NONNULL 74 | #endif 75 | 76 | /** 77 | * \def INLINE Marks a function as to be inlined 78 | */ 79 | #ifdef __GNUC__ 80 | #define INLINE inline 81 | #else 82 | #ifdef _MSC_VER 83 | #define INLINE __inline 84 | #else 85 | #define INLINE inline 86 | #endif 87 | #endif 88 | 89 | /** 90 | * \def EXTERN_INLINE Marks a function as to be inlined, but also externally defined 91 | */ 92 | #define EXTERN_INLINE extern INLINE 93 | 94 | /** 95 | * \def STATIC_INLINE Marks a function as to be inlined, but also statically defined 96 | */ 97 | #define STATIC_INLINE static INLINE 98 | 99 | #endif -------------------------------------------------------------------------------- /conanfile.py: -------------------------------------------------------------------------------- 1 | from conans import ConanFile, CMake 2 | 3 | class HelloConan(ConanFile): 4 | name = "libfixkalman" 5 | version = "20161008" 6 | url = "https://github.com/sunsided/libfixkalman.git" 7 | license = "MIT" 8 | author = "Markus Mayer (widemeadows@gmail.com)" 9 | requires = "libfixmath/20141230@sunside/stable", "libfixmatrix/20140117@sunside/stable" 10 | generators = "cmake" 11 | settings = "os", "compiler", "build_type", "arch" 12 | exports = "*.c", "*.h", "CMakeLists.txt", "*.rst", "AUTHORS", "LICENSE" 13 | 14 | def build(self): 15 | cmake = CMake(self.settings) 16 | self.run('cmake %s %s' % (self.conanfile_directory, cmake.command_line)) 17 | self.run("cmake --build . %s" % cmake.build_config) 18 | 19 | def package(self): 20 | self.copy("*.h", dst="include") 21 | self.copy("*.c", dst="src") 22 | self.copy("*.lib", dst="lib", src="lib") 23 | self.copy("*.a", dst="lib", src="lib") 24 | 25 | def package_info(self): 26 | name = "libfixkalman" 27 | self.cpp_info.libs = ["libfixkalman"] -------------------------------------------------------------------------------- /example_gravity.c: -------------------------------------------------------------------------------- 1 | /*! 2 | * \brief Example of the Kalman filter 3 | * 4 | * In this example, the gravity constant (~9.81 m/s^2) will be estimated using only 5 | * measurements of the position. These measurements have a variance of var(s) = 0.5m. 6 | * 7 | * The formulas used are: 8 | * s = s + v*T + g*0.5*T^2 9 | * v = v + g*T 10 | * g = g 11 | * 12 | * The time constant is set to T = 1s. 13 | * 14 | * The initial estimation of the gravity constant is set to 6 m/s^2. 15 | */ 16 | 17 | #include 18 | 19 | #define KALMAN_TIME_VARYING 20 | #define KALMAN_JOSEPH_FORM 21 | 22 | #include "fixkalman.h" 23 | 24 | /** 25 | * \brief Enables usage of the functions specialized for systems without control inputs 26 | * 27 | * Define to 0 in order to use the regular functions 28 | */ 29 | #define USE_UNCONTROLLED 1 30 | 31 | // create the filter structure 32 | #define KALMAN_NAME gravity 33 | #define KALMAN_NUM_STATES 3 34 | #define KALMAN_NUM_INPUTS 0 35 | 36 | #if USE_UNCONTROLLED 37 | kalman16_uc_t kf; 38 | #else 39 | kalman16_t kf; 40 | #endif 41 | 42 | 43 | // create the measurement structure 44 | #define KALMAN_MEASUREMENT_NAME position 45 | #define KALMAN_NUM_MEASUREMENTS 1 46 | kalman16_observation_t kfm; 47 | 48 | #define matrix_set(matrix, row, column, value) \ 49 | matrix->data[row][column] = value 50 | 51 | #define matrix_set_symmetric(matrix, row, column, value) \ 52 | matrix->data[row][column] = value; \ 53 | matrix->data[column][row] = value 54 | 55 | #ifndef FIXMATRIX_MAX_SIZE 56 | #error FIXMATRIX_MAX_SIZE must be defined and greater or equal to the number of states, inputs and measurements. 57 | #endif 58 | 59 | #if (FIXMATRIX_MAX_SIZE < KALMAN_NUM_STATES) || (FIXMATRIX_MAX_SIZE < KALMAN_NUM_INPUTS) || (FIXMATRIX_MAX_SIZE < KALMAN_NUM_MEASUREMENTS) 60 | #error FIXMATRIX_MAX_SIZE must be greater or equal to the number of states, inputs and measurements. 61 | #endif 62 | 63 | /*! 64 | * \brief Initializes the gravity Kalman filter 65 | */ 66 | static void kalman_gravity_init() 67 | { 68 | /************************************************************************/ 69 | /* initialize the filter structures */ 70 | /************************************************************************/ 71 | #if USE_UNCONTROLLED 72 | kalman_filter_initialize_uc(&kf, KALMAN_NUM_STATES); 73 | #else 74 | kalman_filter_initialize(&kf, KALMAN_NUM_STATES, KALMAN_NUM_INPUTS); 75 | #endif 76 | kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS); 77 | 78 | /************************************************************************/ 79 | /* set initial state */ 80 | /************************************************************************/ 81 | #if USE_UNCONTROLLED 82 | mf16 *x = kalman_get_state_vector_uc(&kf); 83 | #else 84 | mf16 *x = kalman_get_state_vector(&kf); 85 | #endif 86 | x->data[0][0] = 0; // s_i 87 | x->data[1][0] = 0; // v_i 88 | x->data[2][0] = fix16_from_float(6); // g_i 89 | 90 | /************************************************************************/ 91 | /* set state transition */ 92 | /************************************************************************/ 93 | #if USE_UNCONTROLLED 94 | mf16 *A = kalman_get_state_transition_uc(&kf); 95 | #else 96 | mf16 *A = kalman_get_state_transition(&kf); 97 | #endif 98 | 99 | // set time constant 100 | const fix16_t T = fix16_one; 101 | const fix16_t Tsquare = fix16_sq(T); 102 | 103 | // helper 104 | const fix16_t fix16_half = fix16_from_float(0.5); 105 | 106 | // transition of x to s 107 | matrix_set(A, 0, 0, fix16_one); // 1 108 | matrix_set(A, 0, 1, T); // T 109 | matrix_set(A, 0, 2, fix16_mul(fix16_half, Tsquare)); // 0.5 * T^2 110 | 111 | // transition of x to v 112 | matrix_set(A, 1, 0, 0); // 0 113 | matrix_set(A, 1, 1, fix16_one); // 1 114 | matrix_set(A, 1, 2, T); // T 115 | 116 | // transition of x to g 117 | matrix_set(A, 2, 0, 0); // 0 118 | matrix_set(A, 2, 1, 0); // 0 119 | matrix_set(A, 2, 2, fix16_one); // 1 120 | 121 | /************************************************************************/ 122 | /* set covariance */ 123 | /************************************************************************/ 124 | #if USE_UNCONTROLLED 125 | mf16 *P = kalman_get_system_covariance_uc(&kf); 126 | #else 127 | mf16 *P = kalman_get_system_covariance(&kf); 128 | #endif 129 | 130 | matrix_set_symmetric(P, 0, 0, fix16_half); // var(s) 131 | matrix_set_symmetric(P, 0, 1, 0); // cov(s,v) 132 | matrix_set_symmetric(P, 0, 2, 0); // cov(s,g) 133 | 134 | matrix_set_symmetric(P, 1, 1, fix16_one); // var(v) 135 | matrix_set_symmetric(P, 1, 2, 0); // cov(v,g) 136 | 137 | matrix_set_symmetric(P, 2, 2, fix16_one); // var(g) 138 | 139 | /************************************************************************/ 140 | /* set system process noise */ 141 | /************************************************************************/ 142 | #if USE_UNCONTROLLED 143 | mf16 *Q = kalman_get_system_process_noise_uc(&kf); 144 | mf16_fill(Q, F16(0.0001)); 145 | #endif 146 | 147 | /************************************************************************/ 148 | /* set measurement transformation */ 149 | /************************************************************************/ 150 | mf16 *H = kalman_get_observation_transformation(&kfm); 151 | 152 | matrix_set(H, 0, 0, fix16_one); // z = 1*s 153 | matrix_set(H, 0, 1, 0); // + 0*v 154 | matrix_set(H, 0, 2, 0); // + 0*g 155 | 156 | /************************************************************************/ 157 | /* set process noise */ 158 | /************************************************************************/ 159 | mf16 *R = kalman_get_observation_process_noise(&kfm); 160 | 161 | matrix_set(R, 0, 0, fix16_half); // var(s) 162 | } 163 | 164 | // define measurements. 165 | // 166 | // MATLAB source 167 | // ------------- 168 | // s = s + v*T + g*0.5*T^2; 169 | // v = v + g*T; 170 | #define MEAS_COUNT (15) 171 | static fix16_t real_distance[MEAS_COUNT] = { 172 | F16(4.905), 173 | F16(19.62), 174 | F16(44.145), 175 | F16(78.48), 176 | F16(122.63), 177 | F16(176.58), 178 | F16(240.35), 179 | F16(313.92), 180 | F16(397.31), 181 | F16(490.5), 182 | F16(593.51), 183 | F16(706.32), 184 | F16(828.94), 185 | F16(961.38), 186 | F16(1103.6) }; 187 | 188 | // define measurement noise with variance 0.5 189 | // 190 | // MATLAB source 191 | // ------------- 192 | // noise = 0.5^2*randn(15,1); 193 | static fix16_t measurement_error[MEAS_COUNT] = { 194 | F16(0.13442), 195 | F16(0.45847), 196 | F16(-0.56471), 197 | F16(0.21554), 198 | F16(0.079691), 199 | F16(-0.32692), 200 | F16(-0.1084), 201 | F16(0.085656), 202 | F16(0.8946), 203 | F16(0.69236), 204 | F16(-0.33747), 205 | F16(0.75873), 206 | F16(0.18135), 207 | F16(-0.015764), 208 | F16(0.17869) }; 209 | 210 | /*! 211 | * \brief Runs the gravity Kalman filter. 212 | */ 213 | #if USE_UNCONTROLLED 214 | void kalman_gravity_demo() 215 | { 216 | // initialize the filter 217 | kalman_gravity_init(); 218 | 219 | mf16 *x = kalman_get_state_vector_uc(&kf); 220 | mf16 *z = kalman_get_observation_vector(&kfm); 221 | 222 | // filter! 223 | uint_fast16_t i; 224 | for (i = 0; i < MEAS_COUNT; ++i) 225 | { 226 | // prediction. 227 | kalman_predict_uc(&kf); 228 | 229 | // measure ... 230 | fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]); 231 | matrix_set(z, 0, 0, measurement); 232 | 233 | // update 234 | kalman_correct_uc(&kf, &kfm); 235 | } 236 | 237 | // fetch estimated g 238 | const fix16_t g_estimated = x->data[2][0]; 239 | const float value = fix16_to_float(g_estimated); 240 | assert(value > 9.7 && value < 10); 241 | } 242 | #else 243 | void kalman_gravity_demo() 244 | { 245 | // initialize the filter 246 | kalman_gravity_init(); 247 | 248 | mf16 *x = kalman_get_state_vector(&kf); 249 | mf16 *z = kalman_get_observation_vector(&kfm); 250 | 251 | // filter! 252 | uint_fast16_t i; 253 | for (i = 0; i < MEAS_COUNT; ++i) 254 | { 255 | // prediction. 256 | kalman_predict(&kf); 257 | 258 | // measure ... 259 | fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]); 260 | matrix_set(z, 0, 0, measurement); 261 | 262 | // update 263 | kalman_correct(&kf, &kfm); 264 | } 265 | 266 | // fetch estimated g 267 | const fix16_t g_estimated = x->data[2][0]; 268 | const float value = fix16_to_float(g_estimated); 269 | assert(value > 9.7 && value < 10); 270 | } 271 | #endif 272 | 273 | 274 | /*! 275 | * \brief Runs the gravity Kalman filter with lambda tuning. 276 | */ 277 | #if USE_UNCONTROLLED 278 | void kalman_gravity_demo_lambda() 279 | { 280 | // initialize the filter 281 | kalman_gravity_init(); 282 | 283 | mf16 *x = kalman_get_state_vector_uc(&kf); 284 | mf16 *z = kalman_get_observation_vector(&kfm); 285 | 286 | // forcibly increase uncertainty in every prediction step by ~20% (1/lambda^2) 287 | const fix16_t lambda = F16(0.9); 288 | 289 | // filter! 290 | uint_fast16_t i; 291 | for (i = 0; i < MEAS_COUNT; ++i) 292 | { 293 | // prediction. 294 | kalman_predict_tuned_uc(&kf, lambda); 295 | 296 | // measure ... 297 | fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]); 298 | matrix_set(z, 0, 0, measurement); 299 | 300 | // update 301 | kalman_correct_uc(&kf, &kfm); 302 | } 303 | 304 | // fetch estimated g 305 | const fix16_t g_estimated = x->data[2][0]; 306 | const float value = fix16_to_float(g_estimated); 307 | assert(value > 9.7 && value < 10); 308 | } 309 | #else 310 | void kalman_gravity_demo_lambda() 311 | { 312 | // initialize the filter 313 | kalman_gravity_init(); 314 | 315 | mf16 *x = kalman_get_state_vector(&kf); 316 | mf16 *z = kalman_get_observation_vector(&kfm); 317 | 318 | // forcibly increase uncertainty in every prediction step by ~20% (1/lambda^2) 319 | const fix16_t lambda = F16(0.9); 320 | 321 | // filter! 322 | uint_fast16_t i; 323 | for (i = 0; i < MEAS_COUNT; ++i) 324 | { 325 | // prediction. 326 | kalman_predict_tuned(&kf, lambda); 327 | 328 | // measure ... 329 | fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]); 330 | matrix_set(z, 0, 0, measurement); 331 | 332 | // update 333 | kalman_correct(&kf, &kfm); 334 | } 335 | 336 | // fetch estimated g 337 | const fix16_t g_estimated = x->data[2][0]; 338 | const float value = fix16_to_float(g_estimated); 339 | assert(value > 9.7 && value < 10); 340 | } 341 | #endif 342 | 343 | 344 | void main() 345 | { 346 | kalman_gravity_demo(); 347 | kalman_gravity_demo_lambda(); 348 | } -------------------------------------------------------------------------------- /fixkalman.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "fixarray.h" 5 | #include "settings.h" 6 | 7 | #define EXTERN_INLINE_KALMAN INLINE 8 | #include "fixkalman.h" 9 | 10 | // Calculates dest = A + B * s 11 | HOT NONNULL 12 | STATIC_INLINE void mf16_add_scaled(mf16 *dest, const mf16 *RESTRICT a, const mf16 *RESTRICT b, const fix16_t s) 13 | { 14 | int row, column; 15 | 16 | if (dest->columns != a->columns || dest->rows != a->rows) 17 | dest->errors |= FIXMATRIX_DIMERR; 18 | 19 | if (a->columns != b->columns || a->rows != b->rows) 20 | dest->errors |= FIXMATRIX_DIMERR; 21 | 22 | for (row = 0; row < dest->rows; row++) 23 | { 24 | for (column = 0; column < dest->columns; column++) 25 | { 26 | register const fix16_t scaled = fix16_mul(b->data[row][column], s); 27 | register fix16_t sum = fix16_add(a->data[row][column], scaled); 28 | 29 | #ifndef FIXMATH_NO_OVERFLOW 30 | if (sum == fix16_overflow) 31 | dest->errors |= FIXMATRIX_OVERFLOW; 32 | #endif 33 | 34 | dest->data[row][column] = sum; 35 | } 36 | } 37 | } 38 | 39 | // Calculates dest = (A + B) * s 40 | HOT NONNULL 41 | STATIC_INLINE void mf16_add_and_scale(mf16 *dest, const mf16 *RESTRICT a, const mf16 *RESTRICT b, const fix16_t s) 42 | { 43 | int row, column; 44 | 45 | if (dest->columns != a->columns || dest->rows != a->rows) 46 | dest->errors |= FIXMATRIX_DIMERR; 47 | 48 | if (a->columns != b->columns || a->rows != b->rows) 49 | dest->errors |= FIXMATRIX_DIMERR; 50 | 51 | for (row = 0; row < dest->rows; row++) 52 | { 53 | for (column = 0; column < dest->columns; column++) 54 | { 55 | register const fix16_t unscaled = fix16_add(a->data[row][column], b->data[row][column]); 56 | register fix16_t sum = fix16_mul(unscaled, s); 57 | 58 | #ifndef FIXMATH_NO_OVERFLOW 59 | if (sum == fix16_overflow) 60 | dest->errors |= FIXMATRIX_OVERFLOW; 61 | #endif 62 | 63 | dest->data[row][column] = sum; 64 | } 65 | } 66 | } 67 | 68 | // Calculates dest = dest + A * B 69 | HOT NONNULL 70 | STATIC_INLINE void mf16_mul_add(mf16 *dest, const mf16 *RESTRICT a, const mf16 *RESTRICT b) 71 | { 72 | int row, column; 73 | const int 74 | acolumns = a->columns, 75 | drows = dest->rows, 76 | dcolumns = dest->columns; 77 | 78 | // If dest and input matrices alias, we have to use a temp matrix. 79 | mf16 tmp; 80 | fa16_unalias(dest, (void**)&a, (void**)&b, &tmp, sizeof(tmp)); 81 | 82 | dest->errors = a->errors | b->errors; 83 | 84 | if (a->columns != b->rows) 85 | dest->errors |= FIXMATRIX_DIMERR; 86 | 87 | dest->rows = a->rows; 88 | dest->columns = b->columns; 89 | 90 | for (row = drows-1; row >= 0; --row) 91 | { 92 | const fix16_t *aptr = &a->data[row][0]; 93 | const fix16_t *bptr = &b->data[0][0]; 94 | fix16_t *rowptr = &dest->data[row][0]; 95 | 96 | for (column = dcolumns-1; column >= 0; --column) 97 | { 98 | fix16_t value = fa16_dot( 99 | aptr, 1, 100 | bptr + column, FIXMATRIX_MAX_SIZE, 101 | acolumns); 102 | 103 | // fetch and modify current value 104 | rowptr[column] = fix16_add(rowptr[column], value); 105 | 106 | #ifndef FIXMATH_NO_OVERFLOW 107 | // test for overflows 108 | if (rowptr[column] == fix16_overflow) 109 | dest->errors |= FIXMATRIX_OVERFLOW; 110 | #endif 111 | } 112 | } 113 | } 114 | 115 | // Calculates dest = dest +/- A * B 116 | HOT NONNULL 117 | STATIC_INLINE void mf16_mul_sub(mf16 *dest, const mf16 *RESTRICT a, const mf16 *RESTRICT b) 118 | { 119 | int row, column; 120 | const int 121 | acolumns = a->columns, 122 | drows = dest->rows, 123 | dcolumns = dest->columns; 124 | 125 | // If dest and input matrices alias, we have to use a temp matrix. 126 | mf16 tmp; 127 | fa16_unalias(dest, (void**)&a, (void**)&b, &tmp, sizeof(tmp)); 128 | 129 | dest->errors = a->errors | b->errors; 130 | 131 | if (a->columns != b->rows) 132 | dest->errors |= FIXMATRIX_DIMERR; 133 | 134 | dest->rows = a->rows; 135 | dest->columns = b->columns; 136 | 137 | for (row = drows - 1; row >= 0; --row) 138 | { 139 | const fix16_t *aptr = &a->data[row][0]; 140 | const fix16_t *bptr = &b->data[0][0]; 141 | fix16_t *rowptr = &dest->data[row][0]; 142 | 143 | for (column = dcolumns - 1; column >= 0; --column) 144 | { 145 | fix16_t value = fa16_dot( 146 | aptr, 1, 147 | bptr + column, FIXMATRIX_MAX_SIZE, 148 | acolumns); 149 | 150 | // fetch and modify current value 151 | rowptr[column] = fix16_sub(rowptr[column], value); 152 | 153 | #ifndef FIXMATH_NO_OVERFLOW 154 | // test for overflows 155 | if (rowptr[column] == fix16_overflow) 156 | dest->errors |= FIXMATRIX_OVERFLOW; 157 | #endif 158 | } 159 | } 160 | } 161 | 162 | // Calculates dest = dest + (A * B) * s 163 | HOT NONNULL 164 | STATIC_INLINE void mf16_mul_add_scaled(mf16 *dest, const mf16 *RESTRICT a, const mf16 *RESTRICT b, const register fix16_t scale) 165 | { 166 | int row, column; 167 | const int 168 | acolumns = a->columns, 169 | drows = dest->rows, 170 | dcolumns = dest->columns; 171 | 172 | // If dest and input matrices alias, we have to use a temp matrix. 173 | mf16 tmp; 174 | fa16_unalias(dest, (void**)&a, (void**)&b, &tmp, sizeof(tmp)); 175 | 176 | dest->errors = a->errors | b->errors; 177 | 178 | if (a->columns != b->rows) 179 | dest->errors |= FIXMATRIX_DIMERR; 180 | 181 | dest->rows = a->rows; 182 | dest->columns = b->columns; 183 | 184 | for (row = drows - 1; row >= 0; --row) 185 | { 186 | const fix16_t *aptr = &a->data[row][0]; 187 | const fix16_t *bptr = &b->data[0][0]; 188 | fix16_t *rowptr = &dest->data[row][0]; 189 | 190 | for (column = dcolumns - 1; column >= 0; --column) 191 | { 192 | fix16_t value = fa16_dot( 193 | aptr, 1, 194 | bptr + column, FIXMATRIX_MAX_SIZE, 195 | acolumns); 196 | 197 | // fetch and modify current value 198 | rowptr[column] = fix16_add(rowptr[column], fix16_mul(value, scale)); 199 | 200 | #ifndef FIXMATH_NO_OVERFLOW 201 | // test for overflows 202 | if (rowptr[column] == fix16_overflow) 203 | dest->errors |= FIXMATRIX_OVERFLOW; 204 | #endif 205 | } 206 | } 207 | } 208 | 209 | HOT NONNULL 210 | STATIC_INLINE void mf16_mul_and_scale(mf16 *dest, const mf16 *RESTRICT a, const mf16 *RESTRICT b, const register fix16_t scalar) 211 | { 212 | int row, column; 213 | const int 214 | acolumns = a->columns, 215 | drows = dest->rows, 216 | dcolumns = dest->columns; 217 | 218 | // If dest and input matrices alias, we have to use a temp matrix. 219 | mf16 tmp; 220 | fa16_unalias(dest, (void**)&a, (void**)&b, &tmp, sizeof(tmp)); 221 | 222 | dest->errors = a->errors | b->errors; 223 | 224 | if (a->columns != b->rows) 225 | dest->errors |= FIXMATRIX_DIMERR; 226 | 227 | dest->rows = a->rows; 228 | dest->columns = b->columns; 229 | 230 | for (row = drows - 1; row >= 0; --row) 231 | { 232 | const fix16_t *aptr = &a->data[row][0]; 233 | const fix16_t *bptr = &b->data[0][0]; 234 | fix16_t *rowptr = &dest->data[row][0]; 235 | 236 | for (column = dcolumns - 1; column >= 0; --column) 237 | { 238 | fix16_t value = fa16_dot( 239 | aptr, 1, 240 | bptr + column, FIXMATRIX_MAX_SIZE, 241 | acolumns); 242 | 243 | // modify and set current value 244 | rowptr[column] = fix16_mul(value, scalar); 245 | 246 | #ifndef FIXMATH_NO_OVERFLOW 247 | // test for overflows 248 | if (rowptr[column] == fix16_overflow) 249 | dest->errors |= FIXMATRIX_OVERFLOW; 250 | #endif 251 | } 252 | } 253 | } 254 | 255 | /*! 256 | * \brief Calculates A*B*A' 257 | */ 258 | NONNULL 259 | STATIC_INLINE void mf16_mul_abat(mf16 *dest, const mf16 *a, const mf16 *b) 260 | { 261 | mf16_mul(dest, a, b); 262 | mf16_mul_bt(dest, dest, a); 263 | } 264 | 265 | 266 | /*! 267 | * \brief Calculates A*B*A' * scalar 268 | */ 269 | NONNULL 270 | STATIC_INLINE void mf16_mul_abat_s(mf16 *dest, const mf16 *a, const mf16 *b, fix16_t scalar) 271 | { 272 | mf16_mul_bt(dest, b, a); // dest = B * A' 273 | mf16_mul_and_scale(dest, a, dest, scalar); // dest = A * dest * scalar 274 | } 275 | 276 | /*! 277 | * \brief Calculates dest += A*B*A' 278 | */ 279 | NONNULL 280 | STATIC_INLINE void mf16_mul_abat_add(mf16 *dest, const mf16 *a, const mf16 *b) 281 | { 282 | mf16 func_temp = { a->rows, b->rows }; 283 | mf16_mul_bt(&func_temp, b, a); // dest = B * A' 284 | mf16_mul_add(dest, a, &func_temp); // dest += A * dest 285 | 286 | } 287 | 288 | 289 | #ifndef KALMAN_DISABLE_C 290 | 291 | /*! 292 | * \brief Initializes the Kalman Filter 293 | * \param[in] kf The Kalman Filter structure to initialize 294 | * \param[in] num_states The number of state variables 295 | * \param[in] num_inputs The number of input variables 296 | */ 297 | void kalman_filter_initialize(kalman16_t *const kf, uint_fast8_t num_states, uint_fast8_t num_inputs) 298 | { 299 | kf->A.rows = num_states; 300 | kf->A.columns = num_states; 301 | kf->A.errors = 0; 302 | mf16_fill(&kf->A, 0); 303 | 304 | kf->P.rows = num_states; 305 | kf->P.columns = num_states; 306 | kf->P.errors = 0; 307 | mf16_fill(&kf->P, 0); 308 | 309 | kf->B.rows = num_states; 310 | kf->B.columns = num_inputs; 311 | kf->B.errors = 0; 312 | mf16_fill(&kf->B, 0); 313 | 314 | #ifndef KALMAN_TIME_VARYING 315 | 316 | kf->Q.rows = num_states; 317 | kf->Q.columns = num_states; 318 | kf->Q.errors = 0; 319 | mf16_fill(&kf->Q, 0); 320 | 321 | #else 322 | 323 | kf->Q.rows = num_inputs; 324 | kf->Q.columns = num_inputs; 325 | kf->Q.errors = 0; 326 | mf16_fill(&kf->Q, 0); 327 | 328 | #endif 329 | 330 | kf->x.rows = num_states; 331 | kf->x.columns = 1; 332 | kf->x.errors = 0; 333 | mf16_fill(&kf->x, 0); 334 | 335 | kf->u.rows = num_inputs; 336 | kf->u.columns = 1; 337 | kf->u.errors = 0; 338 | mf16_fill(&kf->u, 0); 339 | } 340 | 341 | #endif 342 | 343 | #ifndef KALMAN_DISABLE_UC 344 | 345 | /*! 346 | * \brief Initializes the Kalman Filter 347 | * \param[in] kf The Kalman Filter structure to initialize 348 | * \param[in] num_states The number of state variables 349 | */ 350 | void kalman_filter_initialize_uc(kalman16_uc_t *const kf, uint_fast8_t num_states) 351 | { 352 | kf->A.rows = num_states; 353 | kf->A.columns = num_states; 354 | kf->A.errors = 0; 355 | mf16_fill(&kf->A, 0); 356 | 357 | kf->P.rows = num_states; 358 | kf->P.columns = num_states; 359 | kf->P.errors = 0; 360 | mf16_fill(&kf->P, 0); 361 | 362 | kf->x.rows = num_states; 363 | kf->x.columns = 1; 364 | kf->x.errors = 0; 365 | mf16_fill(&kf->x, 0); 366 | 367 | kf->Q.rows = num_states; 368 | kf->Q.columns = num_states; 369 | kf->Q.errors = 0; 370 | mf16_fill(&kf->Q, 0); 371 | } 372 | 373 | #endif // KALMAN_DISABLE_UC 374 | 375 | /*! 376 | * \brief Sets the measurement vector 377 | * \param[in] kfm The Kalman Filter measurement structure to initialize 378 | * \param[in] num_states The number of states 379 | * \param[in] num_measurements The number of observations 380 | */ 381 | void kalman_observation_initialize(kalman16_observation_t *const kfm, uint_fast8_t num_states, uint_fast8_t num_observations) 382 | { 383 | kfm->H.rows = num_observations; 384 | kfm->H.columns = num_states; 385 | kfm->H.errors = 0; 386 | mf16_fill(&kfm->H, 0); 387 | 388 | kfm->R.rows = num_observations; 389 | kfm->R.columns = num_observations; 390 | kfm->R.errors = 0; 391 | mf16_fill(&kfm->R, 0); 392 | 393 | kfm->z.rows = num_observations; 394 | kfm->z.columns = 1; 395 | kfm->z.errors = 0; 396 | mf16_fill(&kfm->z, 0); 397 | } 398 | 399 | #ifndef KALMAN_DISABLE_C 400 | 401 | /*! 402 | * \brief Performs the time update / prediction step of only the state vector 403 | * \param[in] kf The Kalman Filter structure to predict with. 404 | */ 405 | HOT NONNULL 406 | void kalman_predict_x(register kalman16_t *const kf) 407 | { 408 | // matrices and vectors 409 | const mf16 *RESTRICT const A = &kf->A; 410 | const mf16 *RESTRICT const B = &kf->B; 411 | const mf16 *RESTRICT const u = &kf->u; 412 | mf16 *RESTRICT const x = &kf->x; 413 | 414 | /************************************************************************/ 415 | /* Predict next state using system dynamics */ 416 | /* x = A*x */ 417 | /************************************************************************/ 418 | 419 | // x = A*x 420 | mf16_mul(x, A, x); 421 | 422 | // TODO: Implement more efficient matrix/row vector multiply 423 | 424 | if (B->rows > 0) 425 | { 426 | mf16_mul_add(x, B, u); // x += B*u 427 | } 428 | } 429 | 430 | #endif 431 | 432 | #ifndef KALMAN_DISABLE_UC 433 | 434 | /*! 435 | * \brief Performs the time update / prediction step of only the state vector 436 | * \param[in] kf The Kalman Filter structure to predict with. 437 | */ 438 | HOT NONNULL 439 | void kalman_predict_x_uc(register kalman16_uc_t *const kf) 440 | { 441 | // matrices and vectors 442 | const mf16 *RESTRICT const A = &kf->A; 443 | mf16 *RESTRICT const x = &kf->x; 444 | 445 | /************************************************************************/ 446 | /* Predict next state using system dynamics */ 447 | /* x = A*x */ 448 | /************************************************************************/ 449 | 450 | // x = A*x 451 | mf16_mul(x, A, x); 452 | } 453 | 454 | /*! 455 | * \brief Performs the time update / prediction step of only the state vector 456 | * \param[in] kf The Kalman Filter structure to predict with. 457 | */ 458 | HOT NONNULL 459 | void kalman_cpredict_x_uc(register kalman16_uc_t *const kf, register fix16_t deltaT) 460 | { 461 | // matrices and vectors 462 | const mf16 *RESTRICT const A = &kf->A; 463 | mf16 *RESTRICT const x = &kf->x; 464 | 465 | /************************************************************************/ 466 | /* Predict next state using system dynamics */ 467 | /* x = A*x */ 468 | /************************************************************************/ 469 | 470 | // x = A*x 471 | #if 0 472 | mf16 dx; 473 | mf16_mul(&dx, A, x); 474 | mf16_add_scaled(x, x, &dx, deltaT); 475 | #else 476 | mf16_mul_add_scaled(x, A, x, deltaT); 477 | #endif 478 | } 479 | 480 | #endif // KALMAN_DISABLE_UC 481 | 482 | #ifndef KALMAN_DISABLE_C 483 | 484 | /*! 485 | * \brief Performs the time update / prediction step of only the state covariance matrix 486 | * \param[in] kf The Kalman Filter structure to predict with. 487 | */ 488 | HOT NONNULL 489 | void kalman_predict_P(register kalman16_t *const kf) 490 | { 491 | // matrices and vectors 492 | const mf16 *RESTRICT const A = &kf->A; 493 | const mf16 *RESTRICT const B = &kf->B; 494 | mf16 *RESTRICT const P = &kf->P; 495 | mf16 *RESTRICT const Q = &kf->Q; 496 | 497 | mf16 P_temp = { P->rows, P->columns }; 498 | mf16 BQ_temp = { A->rows, B->columns }; 499 | 500 | /************************************************************************/ 501 | /* Predict next covariance using system dynamics and input */ 502 | /* P = A*P*A' + B*Q*B' if KALMAN_TIME_VARYING is defined */ 503 | /* P = A*P*A' + Q if KALMAN_TIME_VARYING is not defined */ 504 | /************************************************************************/ 505 | 506 | // P = A*P*A' 507 | mf16_mul_abat(P, A, P); // P = A*P*A' 508 | 509 | // TODO: this should work without P_temp 510 | // TODO: extract the code block below 511 | 512 | // P = P + B*Q*B' 513 | if (B->rows > 0) 514 | { 515 | #ifndef KALMAN_TIME_VARYING 516 | mf16_add(P, P, Q); // P += Q 517 | #else 518 | mf16_mul_abat_add(P, B, Q); // P += B*Q*B' 519 | #endif 520 | } 521 | } 522 | 523 | #endif 524 | 525 | #ifndef KALMAN_DISABLE_UC 526 | 527 | /*! 528 | * \brief Performs the time update / prediction step of only the state covariance matrix 529 | * \param[in] kf The Kalman Filter structure to predict with. 530 | */ 531 | HOT NONNULL 532 | void kalman_predict_P_uc(register kalman16_uc_t *const kf) 533 | { 534 | // matrices and vectors 535 | const mf16 *RESTRICT const A = &kf->A; 536 | const mf16 *RESTRICT const Q = &kf->Q; 537 | mf16 *RESTRICT const P = &kf->P; 538 | 539 | /************************************************************************/ 540 | /* Predict next covariance using system dynamics and input */ 541 | /* P = A*P*A' + Q */ 542 | /************************************************************************/ 543 | 544 | // P = A*P*A' 545 | mf16_mul_abat(P, A, P); // P = A*P*A' 546 | mf16_add(P, P, Q); // P += Q 547 | } 548 | 549 | /*! 550 | * \brief Performs the time update / prediction step of only the state covariance matrix 551 | * \param[in] kf The Kalman Filter structure to predict with. 552 | * \param[in] deltaT The time differential (seconds) 553 | */ 554 | HOT NONNULL 555 | void kalman_cpredict_P_uc(register kalman16_uc_t *const kf, register fix16_t deltaT) 556 | { 557 | // matrices and vectors 558 | const mf16 *RESTRICT const A = &kf->A; 559 | const mf16 *RESTRICT const Q = &kf->Q; 560 | mf16 *RESTRICT const P = &kf->P; 561 | 562 | /************************************************************************/ 563 | /* Predict next covariance using system dynamics and input */ 564 | /* P = (A*P*A' + Q) * deltaT */ 565 | /************************************************************************/ 566 | 567 | // P = A*P*A' 568 | mf16 dP; 569 | mf16_mul_abat(&dP, A, P); // dP = A*P*A' 570 | mf16_add_and_scale(P, &dP, Q, deltaT); // P = (dP + Q)*deltaT 571 | } 572 | 573 | #endif // KALMAN_DISABLE_UC 574 | 575 | #ifndef KALMAN_DISABLE_C 576 | #ifndef KALMAN_DISABLE_LAMBDA 577 | 578 | /*! 579 | * \brief Performs the time update / prediction step of only the state covariance matrix 580 | * \param[in] kf The Kalman Filter structure to predict with. 581 | */ 582 | void kalman_predict_P_tuned(register kalman16_t *const kf, fix16_t lambda) 583 | { 584 | // matrices and vectors 585 | const mf16 *RESTRICT const A = &kf->A; 586 | const mf16 *RESTRICT const B = &kf->B; 587 | mf16 *RESTRICT const P = &kf->P; 588 | 589 | mf16 P_temp = { P->rows, P->columns }; 590 | mf16 BQ_temp = { A->rows, B->columns }; 591 | 592 | /************************************************************************/ 593 | /* Calculate lambda */ 594 | /************************************************************************/ 595 | 596 | static fix16_t last_lambda = 1; 597 | static fix16_t inv_lambda_cached = 1; 598 | 599 | register fix16_t inv_lambda = inv_lambda_cached; 600 | if (lambda != last_lambda) 601 | { 602 | last_lambda = lambda; 603 | 604 | // inv_lambda = 1/lambda^2 605 | inv_lambda_cached = inv_lambda = fix16_div(F16(1), fix16_sq(lambda)); 606 | } 607 | 608 | /************************************************************************/ 609 | /* Predict next covariance using system dynamics and input */ 610 | /* P = A*P*A' * 1/lambda^2 + B*Q*B' if KALMAN_TIME_VARYING is defined */ 611 | /* P = A*P*A' * 1/lambda^2 + Q if KALMAN_TIME_VARYING is not defined */ 612 | /************************************************************************/ 613 | 614 | // P = A*P*A' 615 | mf16_mul_abat(P, A, P); // temp = A*P*A' 616 | mf16_mul_s(P, P, inv_lambda); // P *= 1/(lambda^2) 617 | 618 | // TODO: extract the code block below 619 | 620 | // P = P + B*Q*B' 621 | if (B->rows > 0) 622 | { 623 | #ifndef KALMAN_TIME_VARYING 624 | mf16_add(P, P, &kf->Q); // P += Q 625 | #else 626 | mf16_mul_abat_add(P, B, &kf->Q); // temp = B*Q*B' 627 | #endif 628 | } 629 | } 630 | 631 | #endif // KALMAN_DISABLE_LAMBDA 632 | #endif 633 | 634 | #ifndef KALMAN_DISABLE_UC 635 | #ifndef KALMAN_DISABLE_LAMBDA 636 | 637 | /*! 638 | * \brief Performs the time update / prediction step of only the state covariance matrix 639 | * \param[in] kf The Kalman Filter structure to predict with. 640 | */ 641 | HOT NONNULL 642 | void kalman_predict_P_tuned_uc(register kalman16_uc_t *const kf, fix16_t lambda) 643 | { 644 | // matrices and vectors 645 | const mf16 *RESTRICT const A = &kf->A; 646 | const mf16 *RESTRICT const Q = &kf->Q; 647 | mf16 *RESTRICT const P = &kf->P; 648 | 649 | /************************************************************************/ 650 | /* Calculate lambda */ 651 | /************************************************************************/ 652 | 653 | static fix16_t last_lambda = F16(1); 654 | static fix16_t inv_lambda_cached = F16(1); 655 | 656 | register fix16_t inv_lambda = inv_lambda_cached; 657 | if (lambda != last_lambda) 658 | { 659 | last_lambda = lambda; 660 | 661 | // inv_lambda = 1/lambda^2 662 | inv_lambda_cached = inv_lambda = fix16_div(F16(1), fix16_sq(lambda)); 663 | } 664 | 665 | /************************************************************************/ 666 | /* Predict next covariance using system dynamics and input */ 667 | /* P = A*P*A' * 1/lambda^2 + Q */ 668 | /************************************************************************/ 669 | 670 | // P = A*P*A' 671 | mf16_mul_abat_s(P, A, P, inv_lambda); // temp = A*P*A' * 1/(lambda^2) 672 | mf16_add(P, P, Q); // P += Q 673 | } 674 | 675 | #endif // KALMAN_DISABLE_LAMBDA 676 | #endif // KALMAN_DISABLE_UC 677 | 678 | #if !defined(KALMAN_DISABLE_C) || !defined(KALMAN_DISABLE_UC) 679 | 680 | /*! 681 | * \brief Performs the measurement update step. 682 | * \param[in] kf The Kalman Filter structure to correct. 683 | */ 684 | HOT NONNULL 685 | void kalman_correct(kalman16_t *kf, kalman16_observation_t *kfm) 686 | { 687 | mf16 *RESTRICT const x = &kf->x; 688 | mf16 *RESTRICT const P = &kf->P; 689 | const mf16 *RESTRICT const H = &kfm->H; 690 | 691 | static mf16 K, S, y, 692 | temp_HP, // { H->rows, H->columns }; 693 | temp_PHt; // { P->rows, H->columns }; 694 | 695 | /************************************************************************/ 696 | /* Calculate innovation and residual covariance */ 697 | /* y = z - H*x */ 698 | /* S = H*P*H' + R */ 699 | /************************************************************************/ 700 | 701 | // y = z - H*x 702 | mf16_mul(&y, H, x); 703 | mf16_sub(&y, &kfm->z, &y); 704 | 705 | // S = H*P*H' + R 706 | mf16_mul_abat(&S, H, P); // temp = H*P*H' 707 | mf16_add(&S, &kfm->R, &S); // S += R 708 | 709 | /************************************************************************/ 710 | /* Calculate Kalman gain */ 711 | /* K = P*H' * S^-1 */ 712 | /************************************************************************/ 713 | 714 | // K = P*H' * S^-1 715 | mf16_cholesky(&S, &S); 716 | mf16_invert_lt(&S, &S); // Sinv = S^-1 717 | mf16_mul_bt(&temp_PHt, P, H); // temp = P*H' 718 | mf16_mul(&K, &temp_PHt, &S); // K = temp*Sinv 719 | 720 | /************************************************************************/ 721 | /* Correct state prediction */ 722 | /* x = x + K*y */ 723 | /************************************************************************/ 724 | 725 | // x = x + K*y 726 | mf16_mul_add(x, &K, &y); 727 | 728 | #ifndef KALMAN_JOSEPH_FORM 729 | 730 | /************************************************************************/ 731 | /* Correct state covariances */ 732 | /* P = (I-K*H) * P */ 733 | /* = P - K*(H*P) */ 734 | /************************************************************************/ 735 | 736 | // P = P - K*(H*P) 737 | mf16_mul(&temp_HP, H, P); // temp_HP = H*P 738 | mf16_mul_sub(P, &K, &temp_HP); // P -= K*temp_HP 739 | 740 | #else 741 | 742 | /************************************************************************/ 743 | /* Correct state covariances */ 744 | /* Joseph form */ 745 | /* P = (I-K*H)*P*(I-K*H)' + K*R*K' */ 746 | /************************************************************************/ 747 | 748 | mf16_fill(&temp_HP, F16(0)); // temp_HP reset 749 | mf16_fill_diagonal(&temp_HP, fix16_one); // temp_HP to I (identity matrix) 750 | mf16_mul_sub(&temp_HP, &K, H); // temp_HP = (I-K*H) 751 | mf16_mul_abat(P, &temp_HP, P); // P = (I-K*H)*P*(I-K*H)' 752 | mf16_mul_abat_add(P, &K, &kfm->R); // P += K*R*K' 753 | 754 | #endif /* KALMAN_JOSEPH_FORM */ 755 | 756 | } 757 | 758 | #endif 759 | 760 | -------------------------------------------------------------------------------- /fixkalman.h: -------------------------------------------------------------------------------- 1 | #ifndef _KALMAN_H_ 2 | #define _KALMAN_H_ 3 | 4 | #include 5 | #include "compiler.h" 6 | #include "fixmatrix.h" 7 | 8 | /*! 9 | * \def KALMAN_DISABLE_UC Global define to disable functions for systems without control inputs 10 | */ 11 | #ifdef KALMAN_DISABLE_UC 12 | #pragma message("KALMAN_DISABLE_UC defined. Disabling Kalman filter functions for systems without control inputs.") 13 | #endif 14 | 15 | /*! 16 | * \def KALMAN_DISABLE_C Global define to disable functions for systems with control inputs 17 | */ 18 | #ifdef KALMAN_DISABLE_C 19 | #pragma message("KALMAN_DISABLE_C defined. Disabling Kalman filter functions for systems with control inputs.") 20 | #endif 21 | 22 | /*! 23 | * \def KALMAN_DISABLE_LAMBDA Global define to disable certainty tuning. 24 | */ 25 | #ifdef KALMAN_DISABLE_LAMBDA 26 | #pragma message("KALMAN_DISABLE_LAMBDA defined. Disabling Kalman filter functions with certainty tuning.") 27 | #endif 28 | 29 | #if defined(KALMAN_DISABLE_C) && defined(KALMAN_DISABLE_UC) 30 | #error KALMAN_DISABLE_C and KALMAN_DISABLE_UC defined; This removes all Kalman filter functions. Remove one of the defines and rebuild. 31 | #endif 32 | 33 | #ifndef KALMAN_DISABLE_UC 34 | 35 | /*! 36 | * \brief Kalman Filter structure for a system without control inputs 37 | * \see kalman16_observation_t 38 | * \see kalman16_t 39 | */ 40 | typedef struct 41 | { 42 | /*! 43 | * \brief State vector The state transition matrix (#states x #states) 44 | */ 45 | mf16 x; 46 | 47 | /*! 48 | * \brief System matrix (#states x 1) 49 | * \see P 50 | */ 51 | mf16 A; 52 | 53 | /*! 54 | * \brief System covariance matrix (#states x #states) 55 | * \see A 56 | */ 57 | mf16 P; 58 | 59 | /*! 60 | * \brief System process noise matrix (#states x #states) 61 | * \see P 62 | */ 63 | mf16 Q; 64 | 65 | } kalman16_uc_t; 66 | 67 | #endif // KALMAN_DISABLE_UC 68 | 69 | #if !defined(KALMAN_DISABLE_C) || !defined(KALMAN_DISABLE_UC) // some UC methods require this struct 70 | 71 | /*! 72 | * \brief Kalman Filter structure 73 | * \see kalman16_observation_t 74 | * \see kalman16_uc_t 75 | */ 76 | typedef struct 77 | { 78 | /*! 79 | * \brief State vector The state transition matrix (#states x #states) 80 | */ 81 | mf16 x; 82 | 83 | /*! 84 | * \brief System matrix (#states x 1) 85 | * \see P 86 | */ 87 | mf16 A; 88 | 89 | /*! 90 | * \brief System covariance matrix (#states x #states) 91 | * \see A 92 | */ 93 | mf16 P; 94 | 95 | /*! 96 | * \brief Input vector (#inputs x 1) 97 | */ 98 | mf16 u; 99 | 100 | /*! 101 | * \brief Input matrix (#inputs x #inputs) 102 | * \see Q 103 | */ 104 | mf16 B; 105 | 106 | /*! 107 | * \brief Input covariance/uncertainty matrix (#inputs x #inputs) 108 | * \see B 109 | */ 110 | mf16 Q; 111 | 112 | } kalman16_t; 113 | 114 | #endif // KALMAN_DISABLE_C 115 | 116 | /*! 117 | * \brief Kalman Filter measurement structure 118 | * \see kalman16_t 119 | */ 120 | typedef struct 121 | { 122 | /*! 123 | * \brief Measurement vector (#measurements x 1) 124 | */ 125 | mf16 z; 126 | 127 | /*! 128 | * \brief Measurement transformation matrix (#measurements x #states) 129 | * \see R 130 | */ 131 | mf16 H; 132 | 133 | /*! 134 | * \brief Observation process noise covariance matrix (#measurements x #measurements) 135 | * \see H 136 | */ 137 | mf16 R; 138 | 139 | } kalman16_observation_t; 140 | 141 | /************************************************************************/ 142 | /* Helper defines */ 143 | /************************************************************************/ 144 | 145 | /*! 146 | * \def EXTERN_INLINE_KALMAN Helper inline to switch from local inline to extern inline 147 | */ 148 | #ifndef EXTERN_INLINE_KALMAN 149 | #define EXTERN_INLINE_KALMAN EXTERN_INLINE 150 | #endif 151 | 152 | /************************************************************************/ 153 | /* Functions for systems with control inputs */ 154 | /************************************************************************/ 155 | 156 | #ifndef KALMAN_DISABLE_C 157 | 158 | /*! 159 | * \brief Initializes the Kalman Filter 160 | * \param[in] kf The Kalman Filter structure to initialize 161 | * \param[in] num_states The number of state variables 162 | * \param[in] num_inputs The number of input variables 163 | */ 164 | COLD NONNULL 165 | void kalman_filter_initialize(kalman16_t *const kf, uint_fast8_t num_states, uint_fast8_t num_inputs); 166 | 167 | /*! 168 | * \brief Performs the time update / prediction step of only the state vector 169 | * \param[in] kf The Kalman Filter structure to predict with. 170 | * 171 | * \see kalman_predict 172 | * \see kalman_predict_tuned 173 | */ 174 | HOT NONNULL 175 | void kalman_predict_x(register kalman16_t *const kf); 176 | 177 | /*! 178 | * \brief Performs the time update / prediction step of only the state covariance matrix 179 | * \param[in] kf The Kalman Filter structure to predict with. 180 | * 181 | * \see kalman_predict 182 | * \see kalman_predict_P_tuned 183 | */ 184 | HOT NONNULL 185 | void kalman_predict_P(register kalman16_t *const kf); 186 | 187 | #ifndef KALMAN_DISABLE_LAMBDA 188 | 189 | /*! 190 | * \brief Performs the time update / prediction step of only the state covariance matrix 191 | * \param[in] kf The Kalman Filter structure to predict with. 192 | * 193 | * \see kalman_predict_tuned 194 | * \see kalman_predict_P 195 | */ 196 | HOT NONNULL 197 | void kalman_predict_P_tuned(register kalman16_t *const kf, fix16_t lambda); 198 | 199 | #endif // KALMAN_DISABLE_LAMBDA 200 | 201 | /*! 202 | * \brief Performs the time update / prediction step. 203 | * \param[in] kf The Kalman Filter structure to predict with. 204 | * \param[in] lambda Lambda factor (\c 0 < {\ref lambda} <= \c 1) to forcibly reduce prediction certainty. Smaller values mean larger uncertainty. 205 | * 206 | * This call assumes that the input covariance and variables are already set in the filter structure. 207 | * 208 | * \see kalman_predict_x 209 | * \see kalman_predict_P 210 | */ 211 | HOT NONNULL 212 | EXTERN_INLINE_KALMAN void kalman_predict(kalman16_t *kf) 213 | { 214 | /************************************************************************/ 215 | /* Predict next state using system dynamics */ 216 | /* x = A*x */ 217 | /************************************************************************/ 218 | 219 | kalman_predict_x(kf); 220 | 221 | /************************************************************************/ 222 | /* Predict next covariance using system dynamics and input */ 223 | /* P = A*P*A' + B*Q*B' */ 224 | /************************************************************************/ 225 | 226 | kalman_predict_P(kf); 227 | } 228 | 229 | #ifndef KALMAN_DISABLE_LAMBDA 230 | 231 | /*! 232 | * \brief Performs the time update / prediction step. 233 | * \param[in] kf The Kalman Filter structure to predict with. 234 | * \param[in] lambda Lambda factor (\c 0 < {\ref lambda} <= \c 1) to forcibly reduce prediction certainty. Smaller values mean larger uncertainty. 235 | * 236 | * This call assumes that the input covariance and variables are already set in the filter structure. 237 | * 238 | * \see kalman_predict_x 239 | * \see kalman_predict_P_tuned 240 | */ 241 | HOT NONNULL 242 | EXTERN_INLINE_KALMAN void kalman_predict_tuned(kalman16_t *kf, fix16_t lambda) 243 | { 244 | /************************************************************************/ 245 | /* Predict next state using system dynamics */ 246 | /* x = A*x */ 247 | /************************************************************************/ 248 | 249 | kalman_predict_x(kf); 250 | 251 | /************************************************************************/ 252 | /* Predict next covariance using system dynamics and input */ 253 | /* P = A*P*A' * 1/lambda^2 + B*Q*B' */ 254 | /************************************************************************/ 255 | 256 | kalman_predict_P_tuned(kf, lambda); 257 | } 258 | 259 | #endif // KALMAN_DISABLE_LAMBDA 260 | 261 | #endif // KALMAN_DISABLE_C 262 | 263 | /*! 264 | * \brief Performs the measurement update step. 265 | * \param[in] kf The Kalman Filter structure to correct. 266 | */ 267 | HOT NONNULL 268 | void kalman_correct(kalman16_t *kf, kalman16_observation_t *kfm); 269 | 270 | #ifndef KALMAN_DISABLE_C 271 | 272 | /*! 273 | * \brief Gets a pointer to the state vector x. 274 | * \param[in] kf The Kalman Filter structure 275 | * \return The state vector x. 276 | */ 277 | LEAF RETURNS_NONNULL NONNULL HOT PURE 278 | EXTERN_INLINE_KALMAN mf16* kalman_get_state_vector(kalman16_t *kf) 279 | { 280 | return &(kf->x); 281 | } 282 | 283 | /*! 284 | * \brief Gets a pointer to the state transition matrix A. 285 | * \param[in] kf The Kalman Filter structure 286 | * \return The state transition matrix A. 287 | */ 288 | LEAF RETURNS_NONNULL NONNULL HOT PURE 289 | EXTERN_INLINE_KALMAN mf16* kalman_get_state_transition(kalman16_t *kf) 290 | { 291 | return &(kf->A); 292 | } 293 | 294 | /*! 295 | * \brief Gets a pointer to the system covariance matrix P. 296 | * \param[in] kf The Kalman Filter structure 297 | * \return The system covariance matrix. 298 | */ 299 | LEAF RETURNS_NONNULL NONNULL PURE 300 | EXTERN_INLINE_KALMAN mf16* kalman_get_system_covariance(kalman16_t *kf) 301 | { 302 | return &(kf->P); 303 | } 304 | 305 | /*! 306 | * \brief Gets a pointer to the input vector u. 307 | * \param[in] kf The Kalman Filter structure 308 | * \return The input vector u. 309 | */ 310 | LEAF RETURNS_NONNULL NONNULL HOT PURE 311 | EXTERN_INLINE_KALMAN mf16* kalman_get_input_vector(kalman16_t *kf) 312 | { 313 | return &(kf->u); 314 | } 315 | 316 | /*! 317 | * \brief Gets a pointer to the input transition matrix B. 318 | * \param[in] kf The Kalman Filter structure 319 | * \return The input transition matrix B. 320 | */ 321 | LEAF RETURNS_NONNULL NONNULL HOT PURE 322 | EXTERN_INLINE_KALMAN mf16* kalman_get_input_transition(kalman16_t *kf) 323 | { 324 | return &(kf->B); 325 | } 326 | 327 | /*! 328 | * \brief Gets a pointer to the input covariance matrix P. 329 | * \param[in] kf The Kalman Filter structure 330 | * \return The input covariance matrix. 331 | */ 332 | LEAF RETURNS_NONNULL NONNULL HOT PURE 333 | EXTERN_INLINE_KALMAN mf16* kalman_get_input_covariance(kalman16_t *kf) 334 | { 335 | return &(kf->Q); 336 | } 337 | 338 | #endif // KALMAN_DISABLE_C 339 | 340 | /************************************************************************/ 341 | /* Functions for observation handling */ 342 | /************************************************************************/ 343 | 344 | /*! 345 | * \brief Sets the measurement vector 346 | * \param[in] kfm The Kalman Filter measurement structure to initialize 347 | * \param[in] num_states The number of states 348 | * \param[in] num_observations The number of observations 349 | */ 350 | COLD LEAF NONNULL 351 | void kalman_observation_initialize(kalman16_observation_t *const kfm, uint_fast8_t num_states, uint_fast8_t num_observations); 352 | 353 | 354 | /*! 355 | * \brief Gets a pointer to the measurement vector z. 356 | * \param[in] kfm The Kalman Filter measurement structure. 357 | * \return The measurement vector z. 358 | */ 359 | LEAF RETURNS_NONNULL NONNULL HOT PURE 360 | EXTERN_INLINE_KALMAN mf16* kalman_get_observation_vector(kalman16_observation_t *kfm) 361 | { 362 | return &(kfm->z); 363 | } 364 | 365 | /*! 366 | * \brief Gets a pointer to the measurement transformation matrix H. 367 | * \param[in] kfm The Kalman Filter measurement structure. 368 | * \return The measurement transformation matrix H. 369 | */ 370 | RETURNS_NONNULL NONNULL HOT PURE 371 | EXTERN_INLINE_KALMAN mf16* kalman_get_observation_transformation(kalman16_observation_t *kfm) 372 | { 373 | return &(kfm->H); 374 | } 375 | 376 | /*! 377 | * \brief Gets a pointer to the process noise matrix R. 378 | * \param[in] kfm The Kalman Filter measurement structure. 379 | * \return The process noise matrix R. 380 | */ 381 | LEAF RETURNS_NONNULL NONNULL HOT PURE 382 | EXTERN_INLINE_KALMAN mf16* kalman_get_observation_process_noise(kalman16_observation_t *kfm) 383 | { 384 | return &(kfm->R); 385 | } 386 | 387 | /************************************************************************/ 388 | /* Functions for systems without control inputs */ 389 | /************************************************************************/ 390 | 391 | #ifndef KALMAN_DISABLE_UC 392 | 393 | /*! 394 | * \brief Initializes the Kalman Filter 395 | * \param[in] kf The Kalman Filter structure to initialize 396 | * \param[in] num_states The number of state variables 397 | */ 398 | COLD NONNULL 399 | void kalman_filter_initialize_uc(kalman16_uc_t *const kf, uint_fast8_t num_states); 400 | 401 | /*! 402 | * \brief Performs the time update / prediction step of only the state vector 403 | * \param[in] kf The Kalman Filter structure to predict with. 404 | * 405 | * \see kalman_predict_uc 406 | * \see kalman_predict_tuned_uc 407 | */ 408 | HOT NONNULL 409 | void kalman_predict_x_uc(register kalman16_uc_t *const kf); 410 | 411 | /*! 412 | * \brief Performs the time update / prediction step of only the state vector with integration. 413 | * \param[in] kf The Kalman Filter structure to predict with. 414 | * \param[in] deltaT The time differential (seconds) 415 | * 416 | * \see kalman_predict_uc 417 | * \see kalman_predict_tuned_uc 418 | */ 419 | HOT NONNULL 420 | void kalman_cpredict_x_uc(register kalman16_uc_t *const kf, register fix16_t deltaT); 421 | 422 | /*! 423 | * \brief Performs the time update / prediction step of only the state covariance matrix 424 | * \param[in] kf The Kalman Filter structure to predict with. 425 | * 426 | * \see kalman_predict_uc 427 | * \see kalman_predict_P_tuned_uc 428 | */ 429 | HOT NONNULL 430 | void kalman_predict_P_uc(register kalman16_uc_t *const kf); 431 | 432 | /*! 433 | * \brief Performs the continuous-time time update / prediction step of only the state covariance matrix with integration. 434 | * \param[in] kf The Kalman Filter structure to predict with. 435 | * \param[in] deltaT The time differential (seconds) 436 | * 437 | * \see kalman_predict_uc 438 | * \see kalman_predict_P_tuned_uc 439 | */ 440 | HOT NONNULL 441 | void kalman_cpredict_P_uc(register kalman16_uc_t *const kf, register fix16_t deltaT); 442 | 443 | #ifndef KALMAN_DISABLE_LAMBDA 444 | 445 | /*! 446 | * \brief Performs the time update / prediction step of only the state covariance matrix 447 | * \param[in] kf The Kalman Filter structure to predict with. 448 | * 449 | * \see kalman_predict_tuned_uc 450 | * \see kalman_predict_P_uc 451 | */ 452 | HOT NONNULL 453 | void kalman_predict_P_tuned_uc(register kalman16_uc_t *const kf, fix16_t lambda); 454 | 455 | #endif // KALMAN_DISABLE_LAMBDA 456 | 457 | /*! 458 | * \brief Performs the time update / prediction step. 459 | * \param[in] kf The Kalman Filter structure to predict with. 460 | * \param[in] lambda Lambda factor (\c 0 < {\ref lambda} <= \c 1) to forcibly reduce prediction certainty. Smaller values mean larger uncertainty. 461 | * 462 | * This call assumes that the input covariance and variables are already set in the filter structure. 463 | * 464 | * \see kalman_predict_x 465 | * \see kalman_predict_P 466 | */ 467 | NONNULL 468 | EXTERN_INLINE_KALMAN void kalman_predict_uc(kalman16_uc_t *kf) 469 | { 470 | /************************************************************************/ 471 | /* Predict next state using system dynamics */ 472 | /* x = A*x */ 473 | /************************************************************************/ 474 | 475 | kalman_predict_x_uc(kf); 476 | 477 | /************************************************************************/ 478 | /* Predict next covariance using system dynamics and input */ 479 | /* P = A*P*A' + B*Q*B' */ 480 | /************************************************************************/ 481 | 482 | kalman_predict_P_uc(kf); 483 | } 484 | 485 | /*! 486 | * \brief Performs the continous-time filter time update / prediction step with integration. 487 | * \param[in] kf The Kalman Filter structure to predict with. 488 | * \param[in] deltaT The time differential (seconds) 489 | * 490 | * This call assumes that the input covariance and variables are already set in the filter structure. 491 | * 492 | * \see kalman_predict_x 493 | * \see kalman_predict_P 494 | */ 495 | NONNULL 496 | EXTERN_INLINE_KALMAN void kalman_cpredict_uc(kalman16_uc_t *kf, register fix16_t deltaT) 497 | { 498 | /************************************************************************/ 499 | /* Predict next state using system dynamics */ 500 | /* x = A*x */ 501 | /************************************************************************/ 502 | 503 | kalman_cpredict_x_uc(kf, deltaT); 504 | 505 | /************************************************************************/ 506 | /* Predict next covariance using system dynamics and input */ 507 | /* P = A*P*A' + B*Q*B' */ 508 | /************************************************************************/ 509 | 510 | kalman_cpredict_P_uc(kf, deltaT); 511 | } 512 | 513 | #ifndef KALMAN_DISABLE_LAMBDA 514 | 515 | /*! 516 | * \brief Performs the time update / prediction step. 517 | * \param[in] kf The Kalman Filter structure to predict with. 518 | * \param[in] lambda Lambda factor (\c 0 < {\ref lambda} <= \c 1) to forcibly reduce prediction certainty. Smaller values mean larger uncertainty. 519 | * 520 | * This call assumes that the input covariance and variables are already set in the filter structure. 521 | * 522 | * \see kalman_predict_x 523 | * \see kalman_predict_P_tuned 524 | */ 525 | HOT NONNULL 526 | EXTERN_INLINE_KALMAN void kalman_predict_tuned_uc(kalman16_uc_t *kf, fix16_t lambda) 527 | { 528 | /************************************************************************/ 529 | /* Predict next state using system dynamics */ 530 | /* x = A*x */ 531 | /************************************************************************/ 532 | 533 | kalman_predict_x_uc(kf); 534 | 535 | /************************************************************************/ 536 | /* Predict next covariance using system dynamics and input */ 537 | /* P = A*P*A' * 1/lambda^2 + B*Q*B' */ 538 | /************************************************************************/ 539 | 540 | kalman_predict_P_tuned_uc(kf, lambda); 541 | } 542 | 543 | #endif // KALMAN_DISABLE_LAMBDA 544 | 545 | /*! 546 | * \brief Performs the measurement update step. 547 | * \param[in] kf The Kalman Filter structure to correct. 548 | */ 549 | HOT NONNULL 550 | STATIC_INLINE void kalman_correct_uc(kalman16_uc_t *kf, kalman16_observation_t *kfm) 551 | { 552 | // just be careful, kid 553 | kalman_correct((kalman16_t*)kf, kfm); 554 | } 555 | 556 | /*! 557 | * \brief Gets a pointer to the state vector x. 558 | * \param[in] kf The Kalman Filter structure 559 | * \return The state vector x. 560 | */ 561 | LEAF RETURNS_NONNULL NONNULL HOT PURE 562 | EXTERN_INLINE_KALMAN mf16* kalman_get_state_vector_uc(kalman16_uc_t *kf) 563 | { 564 | return &(kf->x); 565 | } 566 | 567 | /*! 568 | * \brief Gets a pointer to the state transition matrix A. 569 | * \param[in] kf The Kalman Filter structure 570 | * \return The state transition matrix A. 571 | */ 572 | LEAF RETURNS_NONNULL NONNULL HOT PURE 573 | EXTERN_INLINE_KALMAN mf16* kalman_get_state_transition_uc(kalman16_uc_t *kf) 574 | { 575 | return &(kf->A); 576 | } 577 | 578 | /*! 579 | * \brief Gets a pointer to the system covariance matrix P. 580 | * \param[in] kf The Kalman Filter structure 581 | * \return The system covariance matrix. 582 | */ 583 | LEAF RETURNS_NONNULL NONNULL PURE 584 | EXTERN_INLINE_KALMAN mf16* kalman_get_system_covariance_uc(kalman16_uc_t *kf) 585 | { 586 | return &(kf->P); 587 | } 588 | 589 | /*! 590 | * \brief Gets a pointer to the system process noise vector. 591 | * \param[in] kf The Kalman Filter structure 592 | * \return The process noise vector. 593 | */ 594 | LEAF RETURNS_NONNULL NONNULL PURE 595 | EXTERN_INLINE_KALMAN mf16* kalman_get_system_process_noise_uc(kalman16_uc_t *kf) 596 | { 597 | return &(kf->Q); 598 | } 599 | 600 | #endif // KALMAN_DISABLE_UC 601 | 602 | #undef EXTERN_INLINE_KALMAN 603 | #endif 604 | -------------------------------------------------------------------------------- /settings.h: -------------------------------------------------------------------------------- 1 | // Settings header for libfixkalman 2 | 3 | #ifndef _SETTINGS_H_ 4 | #define _SETTINGS_H_ 5 | 6 | /* ----------------------------------------------------------------------- */ 7 | /* Replace of the covariance update equation with the Joseph form. */ 8 | /* Set this if Kalman gain is non-optimal or your filter has problems with */ 9 | /* numerical stability. Please be aware that this form of the covariance */ 10 | /* update equation is computationally more expensive. */ 11 | /* ----------------------------------------------------------------------- */ 12 | //#define KALMAN_JOSEPH_FORM 13 | 14 | /* ----------------------------------------------------------------------- */ 15 | /* Transforms the square system process noise matrix into square contol */ 16 | /* input covariance matrix for Kalman filter with control input. */ 17 | /* */ 18 | /* The square system process noise matrix has as many of rows and columns */ 19 | /* as the matrix A. */ 20 | /* In this case the covariance prediction step looks like: P = A*P*A' + Q */ 21 | /* */ 22 | /* By the square contol input covariance matrix the number of rows and */ 23 | /* columns are equal to the number of rows in the input vector u. */ 24 | /* In this case the covariance prediction step looks like: */ 25 | /* P = A*P*A' + B*Q*B' */ 26 | /* ----------------------------------------------------------------------- */ 27 | //#define KALMAN_TIME_VARYING 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /test_package/.gitignore: -------------------------------------------------------------------------------- 1 | /__pycache__/ 2 | -------------------------------------------------------------------------------- /test_package/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.6) 2 | project(PackageTest) 3 | 4 | include(${CMAKE_BINARY_DIR}/conanbuildinfo.cmake) 5 | conan_basic_setup() 6 | 7 | add_executable(package-test main.c) 8 | target_link_libraries(package-test ${CONAN_LIBS}) -------------------------------------------------------------------------------- /test_package/conanfile.py: -------------------------------------------------------------------------------- 1 | from conans import ConanFile, CMake 2 | import os 3 | 4 | channel = os.getenv("CONAN_CHANNEL", "stable") 5 | username = os.getenv("CONAN_USERNAME", "sunside") 6 | 7 | class HelloReuseConan(ConanFile): 8 | settings = "os", "compiler", "build_type", "arch" 9 | requires = "libfixkalman/20161008@%s/%s" % (username, channel) 10 | generators = "cmake" 11 | 12 | def build(self): 13 | cmake = CMake(self.settings) 14 | self.run('cmake "%s" %s' % (self.conanfile_directory, cmake.command_line)) 15 | self.run("cmake --build . %s" % cmake.build_config) 16 | 17 | def test(self): 18 | self.run(os.sep.join([".","bin", "package-test"])) -------------------------------------------------------------------------------- /test_package/main.c: -------------------------------------------------------------------------------- 1 | /*! 2 | * \brief Test project to ensure the package is valid. 3 | */ 4 | 5 | #include 6 | 7 | #define KALMAN_TIME_VARYING 8 | #define KALMAN_JOSEPH_FORM 9 | 10 | #include "fixkalman.h" 11 | 12 | #define KALMAN_NAME gravity 13 | #define KALMAN_NUM_STATES 3 14 | #define KALMAN_NUM_INPUTS 0 15 | 16 | kalman16_uc_t kf; 17 | #define KALMAN_MEASUREMENT_NAME position 18 | #define KALMAN_NUM_MEASUREMENTS 1 19 | kalman16_observation_t kfm; 20 | 21 | #define matrix_set(matrix, row, column, value) \ 22 | matrix->data[row][column] = value 23 | 24 | #define matrix_set_symmetric(matrix, row, column, value) \ 25 | matrix->data[row][column] = value; \ 26 | matrix->data[column][row] = value 27 | 28 | static void kalman_gravity_init() 29 | { 30 | kalman_filter_initialize_uc(&kf, KALMAN_NUM_STATES); 31 | kalman_observation_initialize(&kfm, KALMAN_NUM_STATES, KALMAN_NUM_MEASUREMENTS); 32 | 33 | mf16 *x = kalman_get_state_vector_uc(&kf); 34 | x->data[0][0] = 0; // s_i 35 | x->data[1][0] = 0; // v_i 36 | x->data[2][0] = fix16_from_float(6); // g_i 37 | 38 | mf16 *A = kalman_get_state_transition_uc(&kf); 39 | 40 | const fix16_t T = fix16_one; 41 | const fix16_t Tsquare = fix16_sq(T); 42 | 43 | const fix16_t fix16_half = fix16_from_float(0.5); 44 | 45 | matrix_set(A, 0, 0, fix16_one); // 1 46 | matrix_set(A, 0, 1, T); // T 47 | matrix_set(A, 0, 2, fix16_mul(fix16_half, Tsquare)); // 0.5 * T^2 48 | matrix_set(A, 1, 0, 0); // 0 49 | matrix_set(A, 1, 1, fix16_one); // 1 50 | matrix_set(A, 1, 2, T); // T 51 | matrix_set(A, 2, 0, 0); // 0 52 | matrix_set(A, 2, 1, 0); // 0 53 | matrix_set(A, 2, 2, fix16_one); // 1 54 | 55 | mf16 *P = kalman_get_system_covariance_uc(&kf); 56 | 57 | matrix_set_symmetric(P, 0, 0, fix16_half); // var(s) 58 | matrix_set_symmetric(P, 0, 1, 0); // cov(s,v) 59 | matrix_set_symmetric(P, 0, 2, 0); // cov(s,g) 60 | matrix_set_symmetric(P, 1, 1, fix16_one); // var(v) 61 | matrix_set_symmetric(P, 1, 2, 0); // cov(v,g) 62 | matrix_set_symmetric(P, 2, 2, fix16_one); // var(g) 63 | 64 | mf16 *Q = kalman_get_system_process_noise_uc(&kf); 65 | mf16_fill(Q, F16(0.0001)); 66 | 67 | mf16 *H = kalman_get_observation_transformation(&kfm); 68 | 69 | matrix_set(H, 0, 0, fix16_one); // z = 1*s 70 | matrix_set(H, 0, 1, 0); // + 0*v 71 | matrix_set(H, 0, 2, 0); // + 0*g 72 | 73 | mf16 *R = kalman_get_observation_process_noise(&kfm); 74 | 75 | matrix_set(R, 0, 0, fix16_half); // var(s) 76 | } 77 | 78 | #define MEAS_COUNT (15) 79 | static fix16_t real_distance[MEAS_COUNT] = { 80 | F16(4.905), 81 | F16(19.62), 82 | F16(44.145), 83 | F16(78.48), 84 | F16(122.63), 85 | F16(176.58), 86 | F16(240.35), 87 | F16(313.92), 88 | F16(397.31), 89 | F16(490.5), 90 | F16(593.51), 91 | F16(706.32), 92 | F16(828.94), 93 | F16(961.38), 94 | F16(1103.6) }; 95 | 96 | static fix16_t measurement_error[MEAS_COUNT] = { 97 | F16(0.13442), 98 | F16(0.45847), 99 | F16(-0.56471), 100 | F16(0.21554), 101 | F16(0.079691), 102 | F16(-0.32692), 103 | F16(-0.1084), 104 | F16(0.085656), 105 | F16(0.8946), 106 | F16(0.69236), 107 | F16(-0.33747), 108 | F16(0.75873), 109 | F16(0.18135), 110 | F16(-0.015764), 111 | F16(0.17869) }; 112 | 113 | void kalman_gravity_demo() 114 | { 115 | // initialize the filter 116 | kalman_gravity_init(); 117 | 118 | mf16 *x = kalman_get_state_vector_uc(&kf); 119 | mf16 *z = kalman_get_observation_vector(&kfm); 120 | 121 | // filter! 122 | uint_fast16_t i; 123 | for (i = 0; i < MEAS_COUNT; ++i) 124 | { 125 | // prediction. 126 | kalman_predict_uc(&kf); 127 | 128 | // measure ... 129 | fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]); 130 | matrix_set(z, 0, 0, measurement); 131 | 132 | // update 133 | kalman_correct_uc(&kf, &kfm); 134 | } 135 | 136 | // fetch estimated g 137 | const fix16_t g_estimated = x->data[2][0]; 138 | const float value = fix16_to_float(g_estimated); 139 | assert(value > 9.7 && value < 10); 140 | } 141 | 142 | void kalman_gravity_demo_lambda() 143 | { 144 | // initialize the filter 145 | kalman_gravity_init(); 146 | 147 | mf16 *x = kalman_get_state_vector_uc(&kf); 148 | mf16 *z = kalman_get_observation_vector(&kfm); 149 | 150 | // forcibly increase uncertainty in every prediction step by ~20% (1/lambda^2) 151 | const fix16_t lambda = F16(0.9); 152 | 153 | // filter! 154 | uint_fast16_t i; 155 | for (i = 0; i < MEAS_COUNT; ++i) 156 | { 157 | // prediction. 158 | kalman_predict_tuned_uc(&kf, lambda); 159 | 160 | // measure ... 161 | fix16_t measurement = fix16_add(real_distance[i], measurement_error[i]); 162 | matrix_set(z, 0, 0, measurement); 163 | 164 | // update 165 | kalman_correct_uc(&kf, &kfm); 166 | } 167 | 168 | // fetch estimated g 169 | const fix16_t g_estimated = x->data[2][0]; 170 | const float value = fix16_to_float(g_estimated); 171 | assert(value > 9.7 && value < 10); 172 | } 173 | 174 | void main() 175 | { 176 | kalman_gravity_demo(); 177 | kalman_gravity_demo_lambda(); 178 | } --------------------------------------------------------------------------------