├── ArduiKalman
    ├── ArduiKalman.cpp
    ├── ArduiKalman.h
    ├── mat.h
    └── test_kalman.ino
├── Kalman-filter-equations-and-instruction.png
├── README.md
└── kalman1.png


/ArduiKalman/ArduiKalman.cpp:
--------------------------------------------------------------------------------
 1 | #include "ArduiKalman.h"
 2 | #include <math.h>
 3 | #include <stdio.h>
 4 | #include "mat.h" 
 5 | 
 6 | KalmanFilter::KalmanFilter()
 7 | {}
 8 | 
 9 | void KalmanFilter::init(int nostates, int nobsers, \
10 |                         float *A, float *P, float *Q, float *H, float *R, float *xp, float *xc)
11 | {
12 |   this->m_States = nostates;
13 |   this->m_Obsers = nobsers;
14 |   this->A = A;
15 |   this->P = P;
16 |   this->Q = Q;
17 |   this->H = H;
18 |   this->R = R;
19 |   this->xp = xp;
20 |   this->xc = xc;
21 | }
22 | void KalmanFilter::zeros()
23 | {
24 |   for(int i=0; i<m_States*m_States; i++)
25 |   {
26 |     this->A[i] = 0.0f;
27 |     this->Q[i] = 0.0f;
28 |     this->P[i] = 0.0f;
29 |   }
30 |   for(int i=0; i<m_Obsers*m_States; i++)
31 |   {
32 |     this->H[i] = 0.0f;
33 |   }
34 |   for(int i=0; i<m_Obsers*m_Obsers; i++)
35 |   {
36 |     this->R[i] = 0.0f;
37 |   }
38 |   for(int i=0; i<m_States; i++)
39 |   {
40 |     this->xp[i] = 0.0f;
41 |     this->xc[i] = 0.0f;
42 |   }
43 | }
44 | 
45 | float *KalmanFilter::predict()
46 | {
47 |   mulmat(A, xc, xp, m_States, m_States, 1);
48 |   return xp;
49 | }
50 | 
51 | float *KalmanFilter::correct(float *z)
52 | {  
53 |   float tmp0[m_States*m_States];
54 |   float tmp1[m_States*m_Obsers];
55 |   float tmp2[m_Obsers*m_States];
56 |   float tmp3[m_Obsers*m_Obsers];
57 |   float tmp4[m_Obsers*m_Obsers];
58 |   float tmp5[m_Obsers*m_Obsers];
59 |   float tmp6[m_Obsers]; 
60 |   float Ht[m_States*m_Obsers]; 
61 |   float At[m_States*m_States]; 
62 |   float Pp[m_States*m_States];
63 |   float K[m_States*m_Obsers];
64 | 
65 |   /* P_k = F_{k-1} P_{k-1} F^T_{k-1} + Q_{k-1} */
66 |   mulmat(A, P, tmp0, m_States, m_States, m_States);
67 |   transpose(A, At, m_States, m_States);
68 |   mulmat(tmp0, At, Pp, m_States, m_States, m_States);
69 |   accum(Pp, Q, m_States, m_States);
70 | 
71 |   /* G_k = P_k H^T_k (H_k P_k H^T_k + R)^{-1} */
72 |   transpose(H, Ht, m_Obsers, m_States);
73 |   mulmat(Pp, Ht, tmp1, m_States, m_States, m_Obsers);
74 | 
75 |   mulmat(H, Pp, tmp2, m_Obsers, m_States, m_States);
76 |   mulmat(tmp2, Ht, tmp3, m_Obsers, m_States, m_Obsers);
77 |   accum(tmp3, R, m_Obsers, m_Obsers);
78 | 
79 |   if (cholsl(tmp3, tmp4, tmp5, m_Obsers)) 
80 |   {
81 |     return NULL;
82 |   }
83 |   mulmat(tmp1, tmp4, K, m_States, m_Obsers, m_Obsers);
84 | 
85 |   /* \hat{x}_k = \hat{x_k} + G_k(z_k - h(\hat{x}_k)) */
86 |   mulmat(H, xp, tmp6, m_Obsers, m_States, m_States);
87 |   sub(z, tmp6, tmp5, m_Obsers);
88 |   mulvec(K, tmp5, tmp2, m_States, m_Obsers);
89 |   add(xp, tmp2, xc, m_States);
90 | 
91 |   /* P_k = (I - G_k H_k) P_k */
92 |   mulmat(K, H, tmp0, m_States, m_Obsers, m_States);
93 |   negate(tmp0, m_States, m_States);
94 |   mat_addeye(tmp0, m_States);
95 |   mulmat(tmp0, Pp, P, m_States, m_States, m_States);
96 | 
97 |   return xc;
98 | }


--------------------------------------------------------------------------------
/ArduiKalman/ArduiKalman.h:
--------------------------------------------------------------------------------
 1 | #ifndef __KALMAN_H__
 2 | #define __KALMAN_H__
 3 | 
 4 | class KalmanFilter 
 5 | {
 6 | public:
 7 |   float* A;
 8 |   float* P;
 9 |   float* Q;
10 |   float* H;
11 |   float* R;
12 |   float* xp;
13 |   float* xc;
14 | 
15 |   KalmanFilter();
16 |   
17 |   void init(int nostates, int nobsers, float *A, float *P, float *Q, float *H, float *R, float *xp, float *xc);
18 |   void zeros();
19 |   float *predict();
20 |   float *correct(float *z);
21 | 
22 | 
23 | private:
24 |   int m_States;       
25 |   int m_Obsers;
26 | };
27 | 
28 | #endif


--------------------------------------------------------------------------------
/ArduiKalman/mat.h:
--------------------------------------------------------------------------------
  1 | #ifndef __MAT_H__
  2 | #define __MAT_H__
  3 | 
  4 | #include <math.h> 
  5 | #include <stdio.h>
  6 | 
  7 | 
  8 | void zeros(float * a, int m, int n)
  9 | {
 10 |     int j;
 11 |     for (j=0; j<m*n; ++j)
 12 |         a[j] = 0;
 13 | }
 14 | int choldc1(float * a, float * p, int n) {
 15 |     int i,j,k;
 16 |     float sum;
 17 | 
 18 |     for (i = 0; i < n; i++) {
 19 |         for (j = i; j < n; j++) {
 20 |             sum = a[i*n+j];
 21 |             for (k = i - 1; k >= 0; k--) {
 22 |                 sum -= a[i*n+k] * a[j*n+k];
 23 |             }
 24 |             if (i == j) {
 25 |                 if (sum <= 0) {
 26 |                     return 1; /* error */
 27 |                 }
 28 |                 p[i] = sqrt(sum);
 29 |             }
 30 |             else {
 31 |                 a[j*n+i] = sum / p[i];
 32 |             }
 33 |         }
 34 |     }
 35 | 
 36 |     return 0; /* success */
 37 | }
 38 | 
 39 | int choldcsl(float * A, float * a, float * p, int n) 
 40 | {
 41 |     int i,j,k; float sum;
 42 |     for (i = 0; i < n; i++) 
 43 |         for (j = 0; j < n; j++) 
 44 |             a[i*n+j] = A[i*n+j];
 45 |     if (choldc1(a, p, n)) return 1;
 46 |     for (i = 0; i < n; i++) {
 47 |         a[i*n+i] = 1 / p[i];
 48 |         for (j = i + 1; j < n; j++) {
 49 |             sum = 0;
 50 |             for (k = i; k < j; k++) {
 51 |                 sum -= a[j*n+k] * a[k*n+i];
 52 |             }
 53 |             a[j*n+i] = sum / p[j];
 54 |         }
 55 |     }
 56 | 
 57 |     return 0; /* success */
 58 | }
 59 | 
 60 | 
 61 | int cholsl(float * A, float * a, float * p, int n) 
 62 | {
 63 |     int i,j,k;
 64 |     if (choldcsl(A,a,p,n)) return 1;
 65 |     for (i = 0; i < n; i++) {
 66 |         for (j = i + 1; j < n; j++) {
 67 |             a[i*n+j] = 0.0;
 68 |         }
 69 |     }
 70 |     for (i = 0; i < n; i++) {
 71 |         a[i*n+i] *= a[i*n+i];
 72 |         for (k = i + 1; k < n; k++) {
 73 |             a[i*n+i] += a[k*n+i] * a[k*n+i];
 74 |         }
 75 |         for (j = i + 1; j < n; j++) {
 76 |             for (k = j; k < n; k++) {
 77 |                 a[i*n+j] += a[k*n+i] * a[k*n+j];
 78 |             }
 79 |         }
 80 |     }
 81 |     for (i = 0; i < n; i++) {
 82 |         for (j = 0; j < i; j++) {
 83 |             a[i*n+j] = a[j*n+i];
 84 |         }
 85 |     }
 86 | 
 87 |     return 0; /* success */
 88 | }
 89 | /* C <- A * B */
 90 | void mulmat(float * a, float * b, float * c, int arows, int acols, int bcols)
 91 | {
 92 |     int i, j,l;
 93 | 
 94 |     for(i=0; i<arows; ++i)
 95 |         for(j=0; j<bcols; ++j) {
 96 |             c[i*bcols+j] = 0;
 97 |             for(l=0; l<acols; ++l)
 98 |                 c[i*bcols+j] += a[i*acols+l] * b[l*bcols+j];
 99 |         }
100 | }
101 | 
102 | void mulvec(float * a, float * x, float * y, int m, int n)
103 | {
104 |     int i, j;
105 | 
106 |     for(i=0; i<m; ++i) {
107 |         y[i] = 0;
108 |         for(j=0; j<n; ++j)
109 |             y[i] += x[j] * a[i*n+j];
110 |     }
111 | }
112 | 
113 | void transpose(float * a, float * at, int m, int n)
114 | {
115 |     int i,j;
116 | 
117 |     for(i=0; i<m; ++i)
118 |         for(j=0; j<n; ++j) {
119 |             at[j*m+i] = a[i*n+j];
120 |         }
121 | }
122 | 
123 | /* A <- A + B */
124 | void accum(float * a, float * b, int m, int n)
125 | {        
126 |     int i,j;
127 | 
128 |     for(i=0; i<m; ++i)
129 |         for(j=0; j<n; ++j)
130 |             a[i*n+j] += b[i*n+j];
131 | }
132 | 
133 | /* C <- A + B */
134 | void add(float * a, float * b, float * c, int n)
135 | {
136 |     int j;
137 | 
138 |     for(j=0; j<n; ++j)
139 |         c[j] = a[j] + b[j];
140 | }
141 | 
142 | 
143 | /* C <- A - B */
144 | void sub(float * a, float * b, float * c, int n)
145 | {
146 |     int j;
147 | 
148 |     for(j=0; j<n; ++j)
149 |         c[j] = a[j] - b[j];
150 | }
151 | 
152 | void negate(float * a, int m, int n)
153 | {        
154 |     int i, j;
155 | 
156 |     for(i=0; i<m; ++i)
157 |         for(j=0; j<n; ++j)
158 |             a[i*n+j] = -a[i*n+j];
159 | }
160 | 
161 | void mat_addeye(float * a, int n)
162 | {
163 |     int i;
164 |     for (i=0; i<n; ++i)
165 |         a[i*n+i] += 1;
166 | }
167 | 
168 | #endif


--------------------------------------------------------------------------------
/ArduiKalman/test_kalman.ino:
--------------------------------------------------------------------------------
 1 | #include "ArduiKalman.h"
 2 | 
 3 | #define ADC_PIN 34
 4 | #define UPDATE_TIME  100
 5 | long current; 
 6 | 
 7 | int stateNum = 1;
 8 | int measureNum = 1;
 9 | 
10 | //init matrix for Kalman
11 | float xc[1];        // correct state vector 
12 | float xp[1];        // predict state vector 
13 | float A[1][1];      // prediction error covariance 
14 | float Q[1][1];      // process noise covariance 
15 | float R[1][1];      // measurement error covariance
16 | float H[1][1];      // Measurement model
17 | float P[1][1];      // Post-prediction, pre-update
18 |   
19 | KalmanFilter m_kf;
20 | 
21 | void setup() {
22 |   Serial.begin(115200);
23 |   Serial.print(0);  // To freeze the lower limit
24 |   Serial.print(" ");
25 |   Serial.print(3.3);  // To freeze the upper limit
26 |   Serial.print(" ");
27 |   
28 |   // put your setup code here, to run once:
29 |   m_kf.init(stateNum, measureNum, &A[0][0], &P[0][0], &Q[0][0], &H[0][0], &R[0][0], &xp[0], &xc[0]);
30 |   m_kf.zeros();
31 |   A[0][0] = 1.0f;
32 |   H[0][0] = 1.0f;
33 |   Q[0][0] = 0.01f;
34 |   R[0][0] = 100.0f;
35 |   P[0][0] = 1.0f;
36 | 
37 |   xc[0] = analogRead(ADC_PIN)/4095.0 * 3.3;
38 | }
39 | 
40 | void loop() {
41 |   // predict
42 |   float *predict = m_kf.predict();
43 |   // correct
44 |   float rand_noise = random(-100,100)/100.0;
45 |   float measured_value = analogRead(ADC_PIN)/4095.0 * 3.3 + rand_noise;
46 |   float measurement[measureNum];
47 |   measurement[0] = measured_value;
48 |   float *correct = m_kf.correct(measurement);
49 |   float estimated_value = correct[0];
50 |   
51 |   //update plotter
52 |   if (millis() > current) {
53 |     Serial.print(measured_value);
54 |     Serial.print(",");
55 |     Serial.print(estimated_value);
56 |     Serial.println();
57 |     
58 |     current = millis() + UPDATE_TIME;
59 |   }
60 | 
61 | }
62 | 


--------------------------------------------------------------------------------
/Kalman-filter-equations-and-instruction.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/nhatuan84/Arduino-KalmanFilter/5e2b019c9c9748bfc277a492b0a8e4e04a060ca7/Kalman-filter-equations-and-instruction.png


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # Arduino-KalmanFilter
 2 | Arduino-KalmanFilter
 3 | 
 4 | Based on https://github.com/simondlevy/TinyEKF
 5 | 
 6 | This is a matrix version of Kalman Filter for ESP8266/ESP32/MCUs. 
 7 | 
 8 | It can be applied to Arduino, ESP8266/ESP32/MCUs.
 9 | 
10 | Example for ESP32: https://www.iotsharing.com/2019/06/how-to-apply-kalman-filter-to-esp.html
11 | 
12 | There are 3 files that you need to include in your project (ESP8266/ESP32/MCUs): 
13 | 
14 | - ArduiKalman.cpp
15 | - ArduiKalman.h
16 | - mat.h
17 | 
18 | ![alt text](https://github.com/nhatuan84/Arduino-KalmanFilter/blob/main/Kalman-filter-equations-and-instruction.png)
19 | 
20 | n: number of states
21 | 
22 | m: number of measurement values
23 | 
24 | xc[n]:   correct state vector 
25 | 
26 | xp[n]:   predict state vector 
27 | 
28 | A(n, n): System dynamics matrix
29 | 
30 | H(m, n): Measurement matrix
31 | 
32 | Q(n, n): Process noise covariance
33 | 
34 | R(m, m): Measurement noise covariance
35 | 
36 | P(n, n): Estimate error covariance
37 | 
38 | Predict step:
39 | 
40 |    float *predict = m_kf.predict()
41 |    
42 | Correct step:
43 | 
44 |    float *correct = m_kf.correct(measurement)
45 | 
46 | An example when n=4, m=2
47 | 
48 |   A[0][0] = 1.0f;
49 |   
50 |   A[1][1] = 1.0f;
51 |   
52 |   A[2][2] = 1.0f;
53 |   
54 |   A[3][3] = 1.0f;
55 | 
56 |   H[0][0] = 1.0f;
57 |   
58 |   H[1][1] = 1.0f;
59 | 
60 |   Q[0][0] = 0.01f;
61 |   
62 |   Q[1][1] = 0.01f;
63 |   
64 |   Q[2][2] = 0.01f;
65 |   
66 |   Q[3][3] = 0.01f;
67 | 
68 |   R[0][0] = 0.1f;
69 |   
70 |   R[1][1] = 0.1f;
71 | 
72 |   P[0][0] = 1.0f;
73 |   
74 |   P[1][1] = 1.0f;
75 |   
76 |   P[2][2] = 1.0f;
77 |   
78 |   P[3][3] = 1.0f;
79 |   
80 |   xc[0] = 0.01f;
81 |   
82 |   xc[1] = 0.01f;
83 |   
84 |   xc[2] = 0.01f;
85 |   
86 |   xc[3] = 0.01f;
87 | 
88 | 
89 | ![alt text](https://github.com/nhatuan84/Arduino-KalmanFilter/blob/main/kalman1.png)
90 | 
91 | 


--------------------------------------------------------------------------------
/kalman1.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/nhatuan84/Arduino-KalmanFilter/5e2b019c9c9748bfc277a492b0a8e4e04a060ca7/kalman1.png


--------------------------------------------------------------------------------