├── LICENSE ├── Makefile ├── README.md ├── includes └── eekf │ ├── eekf.h │ └── eekf_mat.h ├── src ├── eekf.c ├── eekf_mat.c └── examples │ ├── eekf_example.c │ └── plot_eekf.m └── toolchain_gcc.mk /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2015 Christian Meißner 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # static library 2 | SRC_LIB := eekf.c eekf_mat.c 3 | TARGET_LIB := libeekf.a 4 | OBJS_LIB := ${SRC_LIB:.c=.o} 5 | 6 | # example program 7 | SRC_EXAMPLE := examples/eekf_example.c 8 | TARGET_EXAMPLE := examples/eekf_example 9 | OBJS_EXAMPLE := ${SRC_EXAMPLE:.c=.o} $(TARGET_LIB) 10 | 11 | # build params 12 | BUILD_DIR := ./build 13 | SRC_DIR := ./src 14 | INCLUDE_DIRS := ./includes 15 | VPATH := src 16 | 17 | include toolchain_gcc.mk 18 | 19 | .PHONY: clean 20 | 21 | all: $(TARGET_LIB) $(TARGET_EXAMPLE) 22 | 23 | # eekf archive 24 | $(TARGET_LIB): $(OBJS_LIB) 25 | @echo "[AR] archiving $@" 26 | @$(AR) $(BUILD_DIR)/$(TARGET_LIB) $(addprefix $(BUILD_DIR)/, $(OBJS_LIB)) 27 | 28 | # example program 29 | $(TARGET_EXAMPLE): $(OBJS_EXAMPLE) 30 | @echo "[LD] linking $@" 31 | @$(CC) -o $(BUILD_DIR)/$(TARGET_EXAMPLE) $(addprefix $(BUILD_DIR)/, $(OBJS_EXAMPLE)) $(LDFLAGS) 32 | 33 | # compile rule 34 | %.o: %.c 35 | @echo "[CC] compiling $@" 36 | @mkdir -p $(BUILD_DIR)/$(dir $@) 37 | @$(CC) $(CFLAGS) -c -o $(BUILD_DIR)/$@ $< 38 | 39 | # clean up rule 40 | clean: 41 | @echo "[CLEAN] cleaning build files" 42 | @$(RM) -r $(BUILD_DIR) 43 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # EEKF - Embedded Extended Kalman Filter 2 | 3 | This project implements an Extended Kalman Filter in C intended for the use in embedded applications. 4 | The main features are: 5 | 6 | - small implementation 7 | - simple C interface using callbacks for state transition and measurement prediction functions 8 | - usable for nonlinear (extended) and linear Kalman Filter cases 9 | - no dynamic memory allocation 10 | - dedicated minimal matrix computation module 11 | - efficient filter computation using Cholesky Factorization 12 | - separated prediction and correction steps 13 | - input and measurment dimension are allowed to change between steps 14 | 15 | ## What is a Kalman Filter? 16 | 17 | With a Kalman Filter one can estimate the internal hidden states of a process/system by measuring only the visible outputs. 18 | This is widely used with Inertia Measurement Units (IMU) to do sensor fusion or dead reconing. As long as no measurement is available the filter will predict the current state of the system. Once a measurement is available it will update the estimate. This is known as prediction and correction step. For more information please refer to http://en.wikipedia.org/wiki/Kalman_filter. 19 | 20 | ## So what is an Extended Kalman Filter? 21 | 22 | In the linear filter case the states from one time step to the next are linear related. That means their exists a constant matrix which expresses that. Also the the output of the system is linear related to the internal states. What if the relations are nonlinear? In this case the relation between the consecutive states and the output is described by nonlinear functions, for which equations of the filter are not solvable. The solution is to linearize the nonlinear functions at the current state and to apply the equations of the linear Kalman. This is what the Extended Kalman filter is all about. For more information please refer to http://en.wikipedia.org/wiki/Kalman_filter. 23 | 24 | ## What the implementation does 25 | 26 | The implementation provides all Kalman Filter computations except for the state prediction function f and the measurment prediction function h. The user has to implemnt these by providing the state and measurement prediction computation and the derivation of the functions with respect to the current filter state (Jacobians). This is done in callbacks. You can use the interface for linear Kalman filter case too. Just let the callbacks return constant Jacobians. The example program shows this approach. 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /includes/eekf/eekf.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/dr-duplo/eekf/2ae17837f96499cb02cf4c5d87a112dfb6fe9ae0/includes/eekf/eekf.h -------------------------------------------------------------------------------- /includes/eekf/eekf_mat.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * The MIT License (MIT) * 3 | * * 4 | * Copyright (c) 2015 Christian Mei�ner * 5 | * * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy * 7 | * of this software and associated documentation files (the "Software"), to deal * 8 | * in the Software without restriction, including without limitation the rights * 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * 10 | * copies of the Software, and to permit persons to whom the Software is * 11 | * furnished to do so, subject to the following conditions: * 12 | * * 13 | * The above copyright notice and this permission notice shall be included in all * 14 | * copies or substantial portions of the Software. * 15 | * * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * 22 | * SOFTWARE. * 23 | ***********************************************************************************/ 24 | 25 | /** 26 | * Basic matrix handling and computation functions. 27 | * 28 | * @copyright The MIT Licence 29 | * @file eekf_mat.h 30 | * @author Christian Mei�ner 31 | */ 32 | 33 | #ifndef EEKF_MAT_H 34 | #define EEKF_MAT_H 35 | 36 | #include 37 | 38 | #define EEKF_MAT_LOG log 39 | #define EEKF_MAT_SQRT sqrt 40 | #define EEKF_MAT_RAND rand 41 | #define EEKF_MAT_RAND_MAX RAND_MAX 42 | 43 | /// matrix value type 44 | typedef double eekf_value; 45 | 46 | /// base matrix structure type 47 | typedef struct { 48 | eekf_value *elements; //!< pointer to matrix elements (column major order) 49 | uint8_t rows; //!< number of rows 50 | uint8_t cols; //!< number of columns 51 | } eekf_mat; 52 | 53 | /// assign data to a matrix 54 | #define EEKF_ASSIGN_MATRIX(matrix, data, rows, cols)\ 55 | do {\ 56 | (matrix).elements = data;\ 57 | (matrix).rows = rows;\ 58 | (matrix).cols = cols;\ 59 | } while(0) 60 | 61 | /// declare a matrix with a non constant size (e.g. function parameter dependent) 62 | #define EEKF_DECL_MAT_DYN(name, rows, cols)\ 63 | eekf_value name##_elements[(rows)*(cols)];\ 64 | eekf_mat name = {name##_elements, (rows), (cols)}; 65 | 66 | /// declare a matrix with given size and value initialization 67 | #define EEKF_DECL_MAT_INIT(name, rows, cols, ...)\ 68 | eekf_value name##_elements[(rows)*(cols)] = {__VA_ARGS__};\ 69 | eekf_mat name = {name##_elements, (rows), (cols)}; 70 | 71 | /// declare a matrix with given size and initialize elements to zero 72 | #define EEKF_DECL_MAT(name, rows, cols) EEKF_DECL_MAT_INIT(name, rows, cols, 0) 73 | 74 | /// get pointer to a given row of a matrix 75 | #define EEKF_MAT_ROW(mat, i) ((mat).elements + (i)) 76 | 77 | /// get pointer to a given col of a matrix 78 | #define EEKF_MAT_COL(mat, j) ((mat).elements + (mat).rows * (j)) 79 | 80 | /// get pointer to a given element of a matrix 81 | #define EEKF_MAT_EL(mat, r, c) ((mat).elements + (c) * (mat).rows + (r)) 82 | 83 | /** 84 | * Multiply two matrices such that C = A * B. 85 | * 86 | * @param [out] C pointer to matrix to hold the result 87 | * @param [in] A pointer to left matrix of multiplication 88 | * @param [in] B pointer to right matrix of multiplication 89 | * @return returns the pointer to result matrix on success, NULL otherwise 90 | */ 91 | eekf_mat* eekf_mat_mul(eekf_mat *C, eekf_mat const *A, eekf_mat const *B); 92 | 93 | /** 94 | * Adds two matrices such that C = A + B. 95 | * 96 | * @param [out] C pointer to matrix to hold the result 97 | * @param [in] A pointer to left matrix of addition 98 | * @param [in] B pointer to right matrix of addition 99 | * @return returns the pointer to result matrix on success, NULL otherwise 100 | */ 101 | eekf_mat* eekf_mat_add(eekf_mat *C, eekf_mat const *A, eekf_mat const *B); 102 | 103 | /** 104 | * Subtracts two matrices such that C = A - B. 105 | * 106 | * @param [out] C pointer to matrix to hold the result 107 | * @param [in] A pointer to left matrix of subtraction 108 | * @param [in] B pointer to right matrix of subtraction 109 | * @return returns the pointer to result matrix on success, NULL otherwise 110 | */ 111 | eekf_mat* eekf_mat_sub(eekf_mat *C, eekf_mat const *A, eekf_mat const *B); 112 | 113 | /** 114 | * Transposes a matrix such that At = A'. 115 | * 116 | * @param [out] At pointer to matrix to hold the result 117 | * @param [in] A pointer to the matrix to be transposed 118 | * @return returns the pointer to result matrix on success, NULL otherwise 119 | */ 120 | eekf_mat* eekf_mat_trs(eekf_mat *At, eekf_mat const *A); 121 | 122 | /** 123 | * Computes the Cholesky Factorization for a matrices. 124 | * 125 | * Computes Cholesky Factorization such that A = L * L' whereat L is a lower 126 | * triangular and A a symmetric positive-definite matrix; 127 | * 128 | * @param [out] L pointer to matrix to hold the result 129 | * @param [in] A pointer to matrix to be factorized 130 | * @return returns the pointer to result matrix on success, NULL otherwise 131 | */ 132 | eekf_mat* eekf_mat_chol(eekf_mat *L, eekf_mat const *A); 133 | 134 | /** 135 | * Computes the Forward Substitution of a linear equation system. 136 | * 137 | * Computes the Forward Substitution of a linear equation system L * X = B such that 138 | * L is a lower triangular matrix and x and b are matrices with equal number of columns. 139 | * 140 | * @param [out] X pointer to matrix to hold the result 141 | * @param [in] L pointer to lower triangular matrix 142 | * @param [in] B pointer to right equation side matrix 143 | * @return returns the pointer to result matrix on success, NULL otherwise 144 | */ 145 | eekf_mat* eekf_mat_fw_sub(eekf_mat *X, eekf_mat const *L, eekf_mat const *B); 146 | 147 | #endif /* EEKF_MAT_H */ 148 | -------------------------------------------------------------------------------- /src/eekf.c: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * The MIT License (MIT) * 3 | * * 4 | * Copyright (c) 2015 Christian Meißner * 5 | * * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy * 7 | * of this software and associated documentation files (the "Software"), to deal * 8 | * in the Software without restriction, including without limitation the rights * 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * 10 | * copies of the Software, and to permit persons to whom the Software is * 11 | * furnished to do so, subject to the following conditions: * 12 | * * 13 | * The above copyright notice and this permission notice shall be included in all * 14 | * copies or substantial portions of the Software. * 15 | * * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * 22 | * SOFTWARE. * 23 | ***********************************************************************************/ 24 | 25 | /** 26 | * Functions to compute filter states of an extended kalman filter. 27 | * 28 | * @copyright The MIT Licence 29 | * @file eekf.c 30 | * @author Christian Meißner 31 | */ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | eekf_return eekf_init(eekf_context *ctx, eekf_mat *x, eekf_mat *P, ekkf_fun_f f, 40 | ekkf_fun_h h, void *userData) 41 | { 42 | if (NULL == ctx || NULL == x || NULL == P || NULL == f || NULL == h 43 | || x->rows != P->rows || x->rows != P->cols) 44 | { 45 | return eEekfReturnParameterError; 46 | } 47 | 48 | // state 49 | ctx->x = x; 50 | ctx->P = P; 51 | 52 | // callbacks 53 | ctx->f = f; 54 | ctx->h = h; 55 | 56 | // user defined data 57 | ctx->userData = userData; 58 | 59 | return eEekfReturnOk; 60 | } 61 | 62 | eekf_return eekf_predict(eekf_context *ctx, eekf_mat const *u, 63 | eekf_mat const *Q) 64 | { 65 | if (NULL == Q || NULL == u || NULL == ctx) 66 | { 67 | return eEekfReturnParameterError; 68 | } 69 | 70 | EEKF_DECL_MAT_DYN(Jf, ctx->x->rows, ctx->x->rows); 71 | EEKF_DECL_MAT_DYN(Jft, Jf.cols, Jf.rows); 72 | EEKF_DECL_MAT_DYN(JfP, Jf.rows, Jf.cols); 73 | EEKF_DECL_MAT_DYN(xp, ctx->x->rows, ctx->x->cols); 74 | 75 | // predict state and linearize system: x1 = f(x,u), Jf = df(x,u)/dx 76 | if (NULL != ctx->f 77 | && eEekfReturnOk != ctx->f(&xp, &Jf, ctx->x, u, ctx->userData)) 78 | { 79 | return eEekfReturnCallbackFailed; 80 | } 81 | // copy prediction to state 82 | memcpy(ctx->x->elements, xp.elements, 83 | sizeof(eekf_value) * ctx->x->rows * ctx->x->cols); 84 | 85 | // predict covariance Pp = A*P*A' + Q 86 | if (NULL 87 | == eekf_mat_add(ctx->P, 88 | eekf_mat_mul(ctx->P, eekf_mat_mul(&JfP, &Jf, ctx->P), 89 | eekf_mat_trs(&Jft, &Jf)), Q)) 90 | { 91 | return eEekfReturnComputationFailed; 92 | } 93 | 94 | return eEekfReturnOk; 95 | } 96 | 97 | eekf_return eekf_correct(eekf_context *ctx, eekf_mat const *z, 98 | eekf_mat const *R) 99 | { 100 | if (NULL == R || NULL == z || NULL == ctx || z->rows != R->rows 101 | || z->rows != R->cols) 102 | { 103 | return eEekfReturnParameterError; 104 | } 105 | 106 | // predicted measurement 107 | EEKF_DECL_MAT_DYN(zp, z->rows, z->cols); 108 | // measurement linearization 109 | EEKF_DECL_MAT_DYN(Jh, z->rows, ctx->x->rows); 110 | // helper matrices 111 | EEKF_DECL_MAT_DYN(PJht, ctx->x->rows, z->rows); 112 | EEKF_DECL_MAT_DYN(L, z->rows, z->rows); 113 | EEKF_DECL_MAT_DYN(U, z->rows, ctx->x->rows); 114 | 115 | // predict measurement and linearize measurement: zp = h(x), Jh = dh(x)/dx 116 | if (NULL != ctx->h 117 | && eEekfReturnOk != ctx->h(&zp, &Jh, ctx->x, ctx->userData)) 118 | { 119 | return eEekfReturnCallbackFailed; 120 | } 121 | 122 | { 123 | EEKF_DECL_MAT_DYN(Ct, Jh.cols, Jh.rows); 124 | // cross covariance 125 | if (NULL == eekf_mat_mul(&PJht, ctx->P, eekf_mat_trs(&Ct, &Jh))) 126 | { 127 | return eEekfReturnComputationFailed; 128 | } 129 | } 130 | 131 | // compute cholesky factorization L of innovation covariance S = (Jh*P*Jh' + R) = L*L' 132 | // for efficient inversion - assumes S is symmetric positive-definite. 133 | { 134 | EEKF_DECL_MAT_DYN(S, R->rows, R->cols); 135 | // cholesky factorization 136 | if (NULL == eekf_mat_chol(&L, eekf_mat_add( // innovation covariance 137 | &S, eekf_mat_mul(&S, &Jh, &PJht), R))) 138 | { 139 | return eEekfReturnComputationFailed; 140 | } 141 | } 142 | 143 | // compute intermediate matrix for computational efficiency 144 | // K = U / L -> U = (L \ PJh')' 145 | { 146 | EEKF_DECL_MAT_DYN(PCtt, PJht.cols, PJht.rows); 147 | EEKF_DECL_MAT_DYN(LPCtt, z->rows, ctx->x->rows); 148 | if (NULL 149 | == eekf_mat_trs(&U, 150 | eekf_mat_fw_sub(&LPCtt, &L, 151 | eekf_mat_trs(&PCtt, &PJht)))) 152 | { 153 | return eEekfReturnComputationFailed; 154 | } 155 | } 156 | 157 | // correct state 158 | // x = xp + U * L \ (z - zp) 159 | { 160 | EEKF_DECL_MAT_DYN(dz, z->rows, z->cols); 161 | EEKF_DECL_MAT_DYN(Ldz, z->rows, ctx->x->cols); 162 | EEKF_DECL_MAT_DYN(cx, ctx->x->rows, ctx->x->cols); 163 | 164 | if (NULL 165 | == eekf_mat_add(ctx->x, ctx->x, 166 | eekf_mat_mul(&cx, &U, 167 | eekf_mat_fw_sub(&Ldz, &L, 168 | eekf_mat_sub(&dz, z, &zp))))) 169 | { 170 | return eEekfReturnComputationFailed; 171 | } 172 | } 173 | 174 | // correct covariance 175 | // P = Pp - U * U' 176 | { 177 | EEKF_DECL_MAT_DYN(Ut, U.cols, U.rows); 178 | EEKF_DECL_MAT_DYN(UUt, ctx->P->rows, ctx->P->cols); 179 | 180 | if (NULL 181 | == eekf_mat_sub(ctx->P, ctx->P, 182 | eekf_mat_mul(&UUt, &U, eekf_mat_trs(&Ut, &U)))) 183 | { 184 | return eEekfReturnComputationFailed; 185 | } 186 | } 187 | 188 | return eEekfReturnOk; 189 | } 190 | 191 | eekf_value eekf_randn() 192 | { 193 | eekf_value x1, x2, w; 194 | do 195 | { 196 | x1 = 2.0 * (eekf_value) EEKF_MAT_RAND() / EEKF_MAT_RAND_MAX - 1.0; 197 | x2 = 2.0 * (eekf_value) EEKF_MAT_RAND() / EEKF_MAT_RAND_MAX - 1.0; 198 | w = x1 * x1 + x2 * x2; 199 | } while (w >= 1.0); 200 | 201 | return x1 * EEKF_MAT_SQRT((-2.0 * EEKF_MAT_LOG(w)) / w); 202 | } 203 | -------------------------------------------------------------------------------- /src/eekf_mat.c: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * The MIT License (MIT) * 3 | * * 4 | * Copyright (c) 2015 Christian Meißner * 5 | * * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy * 7 | * of this software and associated documentation files (the "Software"), to deal * 8 | * in the Software without restriction, including without limitation the rights * 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * 10 | * copies of the Software, and to permit persons to whom the Software is * 11 | * furnished to do so, subject to the following conditions: * 12 | * * 13 | * The above copyright notice and this permission notice shall be included in all * 14 | * copies or substantial portions of the Software. * 15 | * * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * 22 | * SOFTWARE. * 23 | ***********************************************************************************/ 24 | 25 | /** 26 | * Basic matrix handling and computation functions. 27 | * 28 | * @copyright The MIT Licence 29 | * @file eekf_mat.c 30 | * @author Christian Meißner 31 | */ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | 39 | eekf_mat* eekf_mat_mul(eekf_mat *C, eekf_mat const *A, eekf_mat const *B) 40 | { 41 | if ( NULL == C || NULL == A || NULL == B || A->cols != B->rows 42 | || C->rows * C->cols != A->rows * B->cols) 43 | { 44 | return NULL; 45 | } 46 | 47 | uint8_t c; 48 | uint8_t r; 49 | uint8_t i; 50 | eekf_value *res; 51 | eekf_value *value1; 52 | eekf_value *value2; 53 | 54 | memset(C->elements, 0, sizeof(eekf_value) * C->rows * C->cols); 55 | 56 | C->rows = A->rows; 57 | C->cols = B->cols; 58 | 59 | for (c = 0; c < C->cols; c++) 60 | { 61 | for (r = 0; r < C->rows; r++) 62 | { 63 | res = C->elements + c * C->rows + r; 64 | value1 = A->elements + r; 65 | value2 = B->elements + c * B->rows; 66 | for (i = 0; i < A->cols; i++, value1 += A->rows, value2++) 67 | { 68 | *res += *value1 * *value2; 69 | } 70 | } 71 | } 72 | 73 | return C; 74 | } 75 | 76 | eekf_mat* eekf_mat_add(eekf_mat *C, eekf_mat const *A, eekf_mat const *B) 77 | { 78 | if (NULL == C || NULL == A || NULL == B || A->rows != B->rows 79 | || A->cols != B->cols || C->rows != A->rows || C->cols != A->cols) 80 | { 81 | return NULL; 82 | } 83 | 84 | uint16_t N = A->rows * A->cols; 85 | eekf_value *value1 = A->elements; 86 | eekf_value *value2 = B->elements; 87 | eekf_value *res = C->elements; 88 | uint16_t i; 89 | 90 | for (i = 0; i < N; i++, value1++, value2++, res++) 91 | { 92 | *res = *value1 + *value2; 93 | } 94 | 95 | return C; 96 | } 97 | 98 | eekf_mat* eekf_mat_sub(eekf_mat *C, eekf_mat const *A, eekf_mat const *B) 99 | { 100 | if (NULL == C || NULL == A || NULL == B || A->rows != B->rows 101 | || A->cols != B->cols || C->rows != A->rows || C->cols != A->cols) 102 | { 103 | return NULL; 104 | } 105 | 106 | uint16_t N = A->rows * A->cols; 107 | eekf_value *value1 = A->elements; 108 | eekf_value *value2 = B->elements; 109 | eekf_value *res = C->elements; 110 | uint16_t i; 111 | 112 | for (i = 0; i < N; i++, value1++, value2++, res++) 113 | { 114 | *res = *value1 - *value2; 115 | } 116 | 117 | return C; 118 | } 119 | 120 | eekf_mat* eekf_mat_trs(eekf_mat *At, eekf_mat const *A) 121 | { 122 | if (NULL == At || NULL == A || A->rows * A->cols != At->cols * At->rows) 123 | { 124 | return NULL; 125 | } 126 | 127 | uint8_t r, c; 128 | eekf_value *res; 129 | eekf_value *value; 130 | 131 | At->rows = A->cols; 132 | At->cols = A->rows; 133 | 134 | for (c = 0; c < At->cols; c++) 135 | { 136 | res = At->elements + c * At->rows; 137 | value = A->elements + c; 138 | for (r = 0; r < At->rows; r++, res++, value += A->rows) 139 | { 140 | *res = *value; 141 | } 142 | } 143 | 144 | return At; 145 | } 146 | 147 | eekf_mat* eekf_mat_chol(eekf_mat *L, eekf_mat const *A) 148 | { 149 | if (NULL == L || NULL == A || A->rows * A->cols != L->rows * L->cols 150 | || A->rows != A->cols) 151 | { 152 | return NULL; 153 | } 154 | 155 | uint16_t n, N, r, c; 156 | uint16_t offset; 157 | eekf_value *de, *value; 158 | 159 | N = A->cols; 160 | 161 | L->rows = N; 162 | L->cols = N; 163 | 164 | memset(L->elements, 0, sizeof(eekf_value) * N * N); 165 | for (n = 0; n < N; n++) 166 | { 167 | offset = n * N + n; 168 | memcpy(L->elements + offset, A->elements + offset, 169 | sizeof(eekf_value) * (N - n)); 170 | } 171 | 172 | // @see http://www.seas.ucla.edu/~vandenbe/103/lectures/chol.pdf 173 | for (n = 0; n < N; n++) 174 | { 175 | // get diagonal element 176 | de = L->elements + n * N + n; 177 | // check element is positive definite 178 | if (*de <= 0) 179 | { 180 | return NULL; 181 | } 182 | // square root of diagonal element in place 183 | *de = EEKF_MAT_SQRT(*de); 184 | // divide lower column elements by diagonal element 185 | for (r = 1; r < N - n; r++) 186 | { 187 | *(de + r) /= *de; 188 | } 189 | // compose right submatrix 190 | for (c = n + 1; c < N; c++) 191 | { 192 | value = L->elements + c * N + c; 193 | for (r = c; r < N; r++, value++) 194 | { 195 | *value -= *(L->elements + n * N + r) 196 | * *(L->elements + n * N + c); 197 | } 198 | } 199 | } 200 | 201 | return L; 202 | } 203 | 204 | eekf_mat* eekf_mat_fw_sub(eekf_mat *X, eekf_mat const *L, eekf_mat const *B) 205 | { 206 | if (NULL == X || NULL == L || NULL == B || L->rows != B->rows 207 | || X->rows * X->cols != B->cols * L->cols) 208 | { 209 | return NULL; 210 | } 211 | 212 | // set result dimensions 213 | X->rows = L->cols; 214 | X->cols = B->cols; 215 | 216 | // loop vars 217 | uint8_t i, j, k; 218 | eekf_value *b_i, *x_i, *x_j, *diag, *row, *a_ij; 219 | 220 | // loop over cols of x and b 221 | for (k = 0; k < X->cols; k++) 222 | { 223 | // loop over x rows 224 | for (i = 0, x_i = EEKF_MAT_COL(*X, k), b_i = EEKF_MAT_COL(*B, k), diag = 225 | L->elements, row = L->elements; i < X->rows; 226 | i++, diag += L->rows + 1, x_i++, b_i++, row++) 227 | { 228 | *x_i = *b_i; 229 | // substitute up to (excluding) current x 230 | for (j = 0, x_j = EEKF_MAT_COL(*X, k), a_ij = row; j < i; 231 | j++, x_j++, a_ij += L->rows) 232 | { 233 | *x_i -= *a_ij * *x_j; 234 | } 235 | // divide by diagonal element of L 236 | *x_i /= *diag; 237 | } 238 | } 239 | // return result 240 | return X; 241 | } 242 | -------------------------------------------------------------------------------- /src/examples/eekf_example.c: -------------------------------------------------------------------------------- 1 | /*********************************************************************************** 2 | * The MIT License (MIT) * 3 | * * 4 | * Copyright (c) 2015 Christian Meißner * 5 | * * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy * 7 | * of this software and associated documentation files (the "Software"), to deal * 8 | * in the Software without restriction, including without limitation the rights * 9 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * 10 | * copies of the Software, and to permit persons to whom the Software is * 11 | * furnished to do so, subject to the following conditions: * 12 | * * 13 | * The above copyright notice and this permission notice shall be included in all * 14 | * copies or substantial portions of the Software. * 15 | * * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * 21 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * 22 | * SOFTWARE. * 23 | ***********************************************************************************/ 24 | 25 | /** 26 | * Example program that uses the eekf. 27 | * 28 | * @copyright The MIT Licence 29 | * @file eekf_example.c 30 | * @author Christian Meißner 31 | */ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | // constant acceleration 40 | eekf_value a = 0.1; 41 | // time step duration 42 | eekf_value dT = 0.1; 43 | // process noise standard deviation 44 | eekf_value s_w = 0.2; 45 | // measurement noise standard deviation 46 | eekf_value s_z = 10; 47 | 48 | /// the state prediction function: linear case for simplicity 49 | eekf_return transition(eekf_mat* xp, eekf_mat* Jf, eekf_mat const *x, 50 | eekf_mat const *u, void* userData) 51 | { 52 | EEKF_DECL_MAT_INIT(xu, 2, 1, 0); 53 | EEKF_DECL_MAT_INIT(B, 2, 1, dT * dT / 2, dT); 54 | 55 | // the Jacobian of transition() at x 56 | *EEKF_MAT_EL(*Jf, 0, 0) = 1; 57 | *EEKF_MAT_EL(*Jf, 1, 0) = 0; 58 | *EEKF_MAT_EL(*Jf, 0, 1) = dT; 59 | *EEKF_MAT_EL(*Jf, 1, 1) = 1; 60 | 61 | *EEKF_MAT_EL(B, 0, 0) = dT * dT / 2; 62 | *EEKF_MAT_EL(B, 1, 0) = dT; 63 | 64 | // predict state from current state 65 | if (NULL == eekf_mat_add(xp, eekf_mat_mul(xp, Jf, x), eekf_mat_mul(&xu, &B, u))) 66 | { 67 | printf("arg\n"); 68 | return eEekfReturnComputationFailed; 69 | } 70 | 71 | return eEekfReturnOk; 72 | } 73 | 74 | /// the measurement prediction function 75 | eekf_return measurement(eekf_mat* zp, eekf_mat* Jh, eekf_mat const *x, 76 | void* userData) 77 | { 78 | // the Jacobian of measurement() at x 79 | *EEKF_MAT_EL(*Jh, 0, 0) = 1; 80 | *EEKF_MAT_EL(*Jh, 0, 1) = 0; 81 | 82 | // compute the measurement from state x 83 | *EEKF_MAT_EL(*zp, 0, 0) = *EEKF_MAT_EL(*x, 0, 0); 84 | 85 | return eEekfReturnOk; 86 | } 87 | 88 | int main(int argc, char **argv) 89 | { 90 | // filter context 91 | eekf_context ctx; 92 | // state of the filter 93 | EEKF_DECL_MAT_INIT(x, 2, 1, 0); 94 | EEKF_DECL_MAT_INIT(P, 2, 2, 95 | pow(s_w, 2) * pow(dT, 4) / 4, pow(s_w, 2) * pow(dT, 3) / 2, 96 | pow(s_w, 2) * pow(dT, 3) / 2, pow(s_w, 2) * pow(dT, 2)); 97 | // input and process noise variables 98 | EEKF_DECL_MAT_INIT(u, 1, 1, a); 99 | EEKF_DECL_MAT_INIT(Q, 2, 2, 100 | pow(s_w, 2) * pow(dT, 4) / 4, pow(s_w, 2) * pow(dT, 3) / 2, 101 | pow(s_w, 2) * pow(dT, 3) / 2, pow(s_w, 2) * pow(dT, 2)); 102 | // measurement and measurement noise variables 103 | EEKF_DECL_MAT_INIT(z, 1, 1, 0); 104 | EEKF_DECL_MAT_INIT(R, 1, 1, s_z * s_z); 105 | 106 | // initialize the filter context 107 | eekf_init(&ctx, &x, &P, transition, measurement, NULL); 108 | 109 | /* now we are ready to use the filter */ 110 | 111 | // initialize random number generator 112 | srand(0); 113 | 114 | // print out header 115 | printf("k x dx P11 P12 P21 P22 rx rdx z\n"); 116 | // loop over time and present some measurements 117 | int k; 118 | eekf_value v = 0, p = 0; 119 | for (k = 0; k < 1000; k++) 120 | { 121 | // compute virtual measurement 122 | p = *EEKF_MAT_EL(u, 0, 0) / 2.0 * pow(k * dT,2.0); 123 | v = *EEKF_MAT_EL(u, 0, 0) * k * dT; 124 | *EEKF_MAT_EL(z, 0, 0) = p + eekf_randn() * s_z; 125 | 126 | // correct the current filter state 127 | eekf_correct(&ctx, &z, &R); 128 | 129 | // print out 130 | printf("%d %f %f %f %f %f %f %f %f %f\n", k, *EEKF_MAT_EL(*ctx.x, 0, 0), 131 | *EEKF_MAT_EL(*ctx.x, 1, 0), *EEKF_MAT_EL(*ctx.P, 0, 0), 132 | *EEKF_MAT_EL(*ctx.P, 0, 1), *EEKF_MAT_EL(*ctx.P, 1, 0), 133 | *EEKF_MAT_EL(*ctx.P, 1, 1), p, v, *EEKF_MAT_EL(z, 0, 0)); 134 | 135 | *EEKF_MAT_EL(u, 0, 0) = a; 136 | // predict the next filter state 137 | eekf_predict(&ctx, &u, &Q); 138 | } 139 | 140 | return 0; 141 | } 142 | -------------------------------------------------------------------------------- /src/examples/plot_eekf.m: -------------------------------------------------------------------------------- 1 | % invoke example program and store output in eekf.log 2 | system('../../build/examples/eekf_example > ../../build/examples/eekf_example.log'); 3 | 4 | % read eekf.log into D 5 | D = dlmread('../../build/examples/eekf_example.log', ' ', 1, 0); 6 | 7 | % plot data 8 | subplot(3,1,1) 9 | plot(D(:,[2 8 10])) 10 | xlabel('k') 11 | ylabel('position') 12 | legend x p z; 13 | 14 | subplot(3,1,2) 15 | plot(D(:,[3 9])) 16 | xlabel('k') 17 | ylabel('velocity') 18 | legend dx v; 19 | 20 | subplot(3,1,3) 21 | plot(D(:,10) - D(:,8), 'c'); hold on; 22 | plot(D(:,2) - D(:,8), 'r'); hold off; 23 | xlabel('k') 24 | ylabel('error') 25 | legend 'z - p' 'x - p'; 26 | 27 | -------------------------------------------------------------------------------- /toolchain_gcc.mk: -------------------------------------------------------------------------------- 1 | CC = gcc 2 | AR = ar rcs 3 | RM = rm -f 4 | 5 | CFLAGS += -Wall -O2 -std=gnu99 6 | CFLAGS += $(foreach includedir,$(INCLUDE_DIRS),-I$(includedir)) 7 | LDFLAGS += -lm --------------------------------------------------------------------------------