├── Kalman.cpp ├── examples ├── kalman_step │ ├── kalman_step.png │ ├── kalman_step_matplotlib.py │ ├── kalman_step_pyqtgraph.py │ └── kalman_step.ino ├── kalman_minimal │ └── kalman_minimal.ino └── kalman_full │ ├── kalman_full_matplotlib.py │ └── kalman_full.ino ├── library.properties ├── VERSION.md ├── keywords.txt ├── LICENSE ├── README.md ├── GETTING_STARTED.md └── Kalman.h /Kalman.cpp: -------------------------------------------------------------------------------- 1 | // Nothing here... 2 | #include 3 | 4 | 5 | -------------------------------------------------------------------------------- /examples/kalman_step/kalman_step.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rfetick/Kalman/HEAD/examples/kalman_step/kalman_step.png -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Kalman 2 | version=1.1.0 3 | author=Romain Fétick 4 | maintainer=Romain Fétick 5 | sentence=Include Kalman filter to your Arduino projects 6 | paragraph= This library is made to implement the matricial form of the Kalman equations. You can merge noisy data from multiple sensors to get the best estimate of the state of your drone, robot... 7 | category=Data Processing 8 | url=https://github.com/rfetick/Kalman 9 | architectures=* 10 | depends=BasicLinearAlgebra 11 | -------------------------------------------------------------------------------- /VERSION.md: -------------------------------------------------------------------------------- 1 | ## 1.0.3.dev [28 Oct 2019] 2 | 3 | Typographic update: use `this` in class, and `` instead of `"XXX.h"` in includes 4 | 5 | README simplification 6 | 7 | ## 1.0.2 [24 Sep 2019] 8 | 9 | Small update, mainly `library.properties` for Arduino library manager 10 | 11 | ## 1.0.1 [30 Aug 2019] 12 | 13 | Improved SRAM memory saving thanks to `Symmetric` matrices for covariances 14 | 15 | Possibility to use `Symmetric`, `SkewSymmetric`, `TriangulerSup`, `TriangularInf` matrices for `F` 16 | 17 | ## 1.0.0 [24 Aug 2019] 18 | 19 | Creation of the Kalman library 20 | -------------------------------------------------------------------------------- /keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map for Kalman 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | KALMAN KEYWORD1 10 | Symmetric KEYWORD1 11 | SkewSymmetric KEYWORD1 12 | Diagonal KEYWORD1 13 | TriangularSup KEYWORD1 14 | TriangularInf KEYWORD1 15 | 16 | ####################################### 17 | # Methods and Functions (KEYWORD2) 18 | ####################################### 19 | 20 | update KEYWORD2 21 | getxcopy KEYWORD2 22 | 23 | ####################################### 24 | # Constants (LITERAL1) 25 | ####################################### -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Romain JL. FETICK 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 | -------------------------------------------------------------------------------- /examples/kalman_step/kalman_step_matplotlib.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Aug 31 16:16:49 2019 4 | 5 | This code works with 'kalman_step.ino' 6 | > Connect your arduino to Serial port 7 | > Upload the 'kalman_step.ino' to your arduino 8 | > Eventually modify the port name "COM7" below to match yours 9 | > Run this code while your arduino transmits data 10 | 11 | To run this correctly you need the Python libraries Serial, Numpy and Matplotlib 12 | 13 | @author: rfetick 14 | """ 15 | 16 | from serial import Serial 17 | import matplotlib.pyplot as plt 18 | import numpy as np 19 | import re 20 | 21 | port = "COM7" 22 | baudrate = 57600 23 | 24 | Nstate = 2 25 | Nobs = 2 26 | 27 | N = 500 # number of measurements 28 | T = np.arange(N,dtype=float) 29 | 30 | STATE = np.zeros((N,Nstate)) 31 | OBS = np.zeros((N,Nobs)) 32 | KAL = np.zeros((N,Nstate)) 33 | 34 | with Serial(port=port, baudrate=baudrate, timeout=1, writeTimeout=1) as port_serie: 35 | if port_serie.isOpen(): 36 | port_serie.flush() 37 | for i in range(N): 38 | ligne = port_serie.readline() 39 | if((i%50)==0): 40 | print("Iteration %u / %u"%(i,N)) 41 | try: 42 | temp = str(ligne)[2:-5] 43 | temp = re.findall('[-.0-9]+',temp) 44 | temp = [float(t) for t in temp] 45 | #print(temp) 46 | STATE[i,:] = temp[0:Nstate] 47 | OBS[i,:] = temp[Nstate:Nstate+Nobs] 48 | KAL[i,:] = temp[Nstate+Nobs:] 49 | 50 | except: 51 | print("Exception: %s"%ligne) 52 | port_serie.close() 53 | 54 | 55 | #%% PLOT RESULTS 56 | 57 | plt.figure(1) 58 | plt.clf() 59 | 60 | for i in range(2): 61 | plt.subplot(1,2,i+1) 62 | plt.plot(STATE[:,i],linewidth=4,label='True') 63 | plt.scatter(T,OBS[:,i],alpha=0.8,label='Obs') 64 | plt.plot(KAL[:,i],linewidth=2,label='Kalman') 65 | plt.legend() 66 | plt.xlabel("Time [iter]") 67 | plt.ylabel("Data") 68 | plt.grid() 69 | plt.xlim(0,N) 70 | plt.ylim(-1,2) 71 | -------------------------------------------------------------------------------- /examples/kalman_minimal/kalman_minimal.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * This code is a minimal empty shell. 3 | * Fill it and modify it to match your own case. 4 | * 5 | * Author: 6 | * R.JL. Fétick 7 | * 8 | * Revision: 9 | * 24 Aug 2019 - Creation 10 | */ 11 | 12 | #include 13 | using namespace BLA; 14 | 15 | //------------------------------------ 16 | /**** KALMAN PARAMETERS ****/ 17 | //------------------------------------ 18 | 19 | // Dimensions of the matrices 20 | #define Nstate 2 // length of the state vector 21 | #define Nobs 2 // length of the measurement vector 22 | 23 | // measurement std (to be characterized from your sensors) 24 | #define n1 0.2 // noise on the 1st measurement component 25 | #define n2 0.1 // noise on the 2nd measurement component 26 | 27 | // model std (~1/inertia). Freedom you give to relieve your evolution equation 28 | #define m1 0.01 29 | #define m2 0.02 30 | 31 | KALMAN K; // your Kalman filter 32 | BLA::Matrix obs; // observation vector 33 | 34 | // Note: I made 'obs' a global variable so memory is allocated before the loop. 35 | // This might provide slightly better speed efficiency in loop. 36 | 37 | 38 | //----------------------------------- 39 | /**** SETUP ****/ 40 | //----------------------------------- 41 | 42 | void setup() { 43 | 44 | Serial.begin(57600); 45 | 46 | // example of evolution matrix. Size is 47 | K.F = {1.0, 0.0, 48 | 0.0, 1.0}; 49 | // example of measurement matrix. Size is 50 | K.H = {1.0, 0.0, 51 | 0.0, 1.0}; 52 | // example of measurement covariance matrix. Size is 53 | K.R = {n1*n1, 0.0, 54 | 0.0, n2*n2}; 55 | // example of model covariance matrix. Size is 56 | K.Q = {m1*m1, 0.0, 57 | 0.0, m2*m2}; 58 | 59 | } 60 | 61 | //----------------------------------- 62 | /**** LOOP ****/ 63 | //----------------------------------- 64 | 65 | void loop() { 66 | 67 | // eventually update your evolution matrix inside the loop 68 | K.F = {1.0, 0.2, 69 | 0.0, 1.0}; 70 | 71 | // GRAB MEASUREMENT and WRITE IT INTO 'obs' 72 | get_sensor_data(); 73 | 74 | // APPLY KALMAN FILTER 75 | K.update(obs); 76 | 77 | // PRINT RESULTS: measures and estimated state 78 | Serial << obs << ' ' << K.x << '\n'; 79 | } 80 | 81 | //----------------------------------- 82 | /**** GET SENSOR DATA ****/ 83 | //----------------------------------- 84 | 85 | void get_sensor_data(){ 86 | // It is your job to fill in this method 87 | // grab data from your accelerometer, GPS, etc... 88 | obs(0) = 1.0; // some dummy measurement 89 | obs(1) = 0.0; // some dummy measurement 90 | } 91 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kalman ![bdg](https://img.shields.io/github/license/rfetick/Kalman) ![bdg](https://img.shields.io/github/v/release/rfetick/Kalman) ![bdg](https://img.shields.io/github/commits-since/rfetick/Kalman/latest) 2 | 3 | **Implement Kalman filter for your Arduino projects** 4 | 5 | - Github project's page: https://github.com/rfetick/Kalman 6 | 7 | - Tested successfully on _Arduino Uno_ and _Nano_ (ATmega 328P old bootloader) 8 | 9 | - Any suggestion or issue? Please first check the issue section below and then write to https://github.com/rfetick/Kalman/issues 10 | 11 | ## I. Long description 12 | 13 | Other Kalman libraries already exist for Arduino, but so far I have only seen filters applied to independent scalars. The matricial implementation of this project allows to use the full power of the Kalman filter to coupled variables. It allows to merge measurements from multiple sensors such as accelerometers, GPS, ultrasound (distance) or pressure (altitude) sensors... 14 | 15 | This library is adapted to your most sophisticated projects. In order to use it you need some knowledge about matrix formalism and be able to write (or find on internet) the actual state equations of your system. 16 | 17 | ## II. Using the Kalman library 18 | 19 | ### 1. Prerequisites 20 | 21 | `BasicLinearAlgebra`: You might find it in the library manager of your Arduino IDE, or directly download it at https://github.com/tomstewart89/BasicLinearAlgebra 22 | 23 | > [!WARNING] 24 | > It seems that the Kalman library is not compatible with the version `4` of `BasicLinearAlgebra`. You might need to install version `3.XX`. See [issue#8](https://github.com/rfetick/Kalman/issues/8) and [issue#9](https://github.com/rfetick/Kalman/issues/9). 25 | 26 | ### 2. Downloading the Kalman library 27 | 28 | This library is available in the official Arduino library manager. Just type `Kalman` and you should find it. 29 | 30 | Other possibility is to download (or clone) this project from GITHUB and add it to your `Arduino/libraries/` folder. 31 | 32 | ### 3. Start using the library in your Arduino projects 33 | 34 | See the [GETTING_STARTED](GETTING_STARTED.md) file. 35 | 36 | See also the `examples\` folder. 37 | 38 | ## III. Possible issues 39 | 40 | * The library `BLA::Matrix` seems to throw errors for matrices of size `<1,1>`. So the Kalman library will only work for `Nstate>1` and `Nobs>1`. For one-dimensional Kalman filters, please refer to other Arduino libraries. 41 | 42 | * In case of issues with matrices computation please make sure to use the latest version of `BasicLinearAlgebra`. It works fine on my side with `BasicLinearAlgebra` version `3.2`, however compatibilities issues may occur with different versions. 43 | 44 | * Compilation issues with ESP32. Please see [issue#10](https://github.com/rfetick/Kalman/issues/10) 45 | 46 | * Size of matrices has to be relatively small due to the limited SRAM memory of Arduino. Effort has been made to reduce SRAM usage. 47 | 48 | * An issue has been reported with Arduino Nano IoT 33: the program compiles but is not correctly loaded to Arduino Nano IoT 33. I might investigate this in the future. 49 | -------------------------------------------------------------------------------- /examples/kalman_full/kalman_full_matplotlib.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sat Aug 31 17:16:57 2019 4 | 5 | This code works with 'kalman_full.ino' 6 | > Connect your arduino to Serial port 7 | > Upload the 'kalman_full.ino' to your arduino 8 | > Eventually modify the port name "COM7" below to match yours 9 | > Run this code while your arduino transmits data 10 | 11 | To run this correctly you need the Python libraries Serial, Numpy and Matplotlib 12 | 13 | 14 | @author: rfetick 15 | """ 16 | 17 | 18 | from serial import Serial 19 | import matplotlib.pyplot as plt 20 | import numpy as np 21 | import re 22 | 23 | port = "COM7" 24 | baudrate = 57600 25 | 26 | Nstate = 3 27 | Nobs = 2 28 | 29 | N = 300 # number of measurements 30 | T = np.arange(N,dtype=float) 31 | TT = np.concatenate((T,T[::-1])) 32 | 33 | STATE = np.zeros((N,Nstate)) 34 | OBS = np.zeros((N,Nobs)) 35 | KAL = np.zeros((N,Nstate)) 36 | P = np.zeros((N,Nstate**2)) 37 | 38 | with Serial(port=port, baudrate=baudrate, timeout=1, writeTimeout=1) as port_serie: 39 | if port_serie.isOpen(): 40 | port_serie.flush() 41 | for i in range(50): 42 | ligne = port_serie.readline() # remove first data 43 | for i in range(N): 44 | ligne = port_serie.readline() 45 | if((i%50)==0): 46 | print("Iteration %u / %u"%(i,N)) 47 | try: 48 | temp = str(ligne)[2:-5] 49 | temp = re.findall('[-.0-9]+',temp) 50 | temp = [float(t) for t in temp] 51 | #print(temp) 52 | STATE[i,:] = temp[0:Nstate] 53 | OBS[i,:] = temp[Nstate:Nstate+Nobs] 54 | KAL[i,:] = temp[Nstate+Nobs:2*Nstate+Nobs] 55 | P[i,:] = temp[2*Nstate+Nobs:] 56 | 57 | except: 58 | print("Exception: %s"%ligne) 59 | port_serie.close() 60 | 61 | 62 | #%% PLOT RESULTS 63 | 64 | plt.figure(1) 65 | plt.clf() 66 | 67 | ax1 = plt.subplot(311) 68 | PP = np.concatenate((KAL[:,0]+np.sqrt(P[:,0]),KAL[::-1,0]-np.sqrt(P[::-1,0]))) 69 | plt.fill(TT,PP,color="C1",alpha=0.3) 70 | plt.plot(STATE[:,0],linewidth=4,label='True') 71 | plt.scatter(T,OBS[:,0],alpha=0.8,color="C2",label='Obs') 72 | plt.plot(KAL[:,0],linewidth=2,label='Kalman') 73 | plt.legend() 74 | plt.xlabel("Time [iter]") 75 | plt.ylabel("Position") 76 | plt.grid() 77 | plt.xlim(0,N) 78 | plt.ylim(-1.5,1.5) 79 | plt.tight_layout() 80 | 81 | plt.subplot(312, sharex=ax1) 82 | PP = np.concatenate((KAL[:,1]+np.sqrt(P[:,4]),KAL[::-1,1]-np.sqrt(P[::-1,4]))) 83 | plt.fill(TT,PP,color="C1",alpha=0.3) 84 | plt.plot(STATE[:,1],linewidth=4,label='True') 85 | #plt.scatter(T,OBS[:,0],alpha=0.8,label='Obs') 86 | plt.plot(KAL[:,1],linewidth=2,label='Kalman') 87 | plt.legend() 88 | plt.xlabel("Time [iter]") 89 | plt.ylabel("Speed") 90 | plt.grid() 91 | plt.xlim(0,N) 92 | plt.ylim(-5,5) 93 | 94 | plt.subplot(313, sharex=ax1) 95 | PP = np.concatenate((KAL[:,2]+np.sqrt(P[:,-1]),KAL[::-1,2]-np.sqrt(P[::-1,-1]))) 96 | plt.fill(TT,PP,color="C1",alpha=0.3) 97 | plt.plot(STATE[:,2],linewidth=4,label='True') 98 | plt.scatter(T,OBS[:,1],alpha=0.8,color="C2",label='Obs') 99 | plt.plot(KAL[:,2],linewidth=2,label='Kalman') 100 | plt.legend() 101 | plt.xlabel("Time [iter]") 102 | plt.ylabel("Acceleration") 103 | plt.grid() 104 | plt.xlim(0,N) 105 | plt.ylim(-15,15) -------------------------------------------------------------------------------- /GETTING_STARTED.md: -------------------------------------------------------------------------------- 1 | ## Start using the library in your Arduino projects 2 | 3 | ### Equations 4 | 5 | Your state evolution model and state observation are respectively given by the matrix equations 6 | 7 |       xk = Fk xk-1 + Bk uk + qk 8 | 9 |       yk = Hk xk + rk 10 | 11 | where _k_ is the time step, _x_ the state vector, and _y_ the measurement vector. The full list of definitions and respective dimensions are summarized in the table below. 12 | 13 | | NAME | DIMENSION | DEFINITION | 14 | |------|-----------------|----------------------------------| 15 | | x | Nstate | State vector | 16 | | F | Nstate x Nstate | Time evolution matrix | 17 | | B | Nstate x Ncom | Command matrix (optional) | 18 | | u | Ncom | Command vector | 19 | | Q | Nstate x Nstate | Model covariance (~1/inertia) | 20 | | y | Nobs | Observation vector (measurement) | 21 | | H | Nobs x State | Observation matrix | 22 | | R | Nobs x Nobs | Noise covariance | 23 | 24 | ### Examples 25 | 26 | Examples are provided in the so-called `examples/` subfolder. Open them to get an idea on how to use the Kalman library. 27 | 28 | * `kalman_minimal` : Empty shell ready to be filled with your state equations and measurements 29 | 30 | * `kalman_step` : Numerical simulation of a noisy measurement on a step function so you do not need to plug any components to play with the Kalman library. Also includes a Python file to grab the measurements from Arduino and plot them. See figure below. 31 | 32 | ![alt text](examples/kalman_step/kalman_step.png "Kalman filter applied to noisy data") 33 | 34 | _The figure above shows two examples of Kalman filter (blue) applied on noisy data (green). The true state (red) is a step function. On the left graph, components of_ Q _are chosen small (high inertia). Noise is efficiently filtered but response time is longer with respect to the right graph._ 35 | 36 | ### Detailed explanations 37 | 38 | Using the Kalman library in your Arduino files is (I hope) straightforward. First include the library in your `.ino` file. Also use the `BLA` (BasicLinearAlgebra) namespace since you will need to define some BLA vectors 39 | ```cpp 40 | #include "Kalman.h" 41 | using namespace BLA 42 | ``` 43 | 44 | Then define your number of states, number of observations and eventually number of commands. For the sake of simplicity, we will start without any command 45 | ```cpp 46 | #define Nstate 2 47 | #define Nobs 2 48 | ``` 49 | 50 | You can now define a Kalman filter and an observation vector 51 | ```cpp 52 | KALMAN K; 53 | BLA::Matrix obs; 54 | ``` 55 | 56 | In the `setup` and `loop` functions you can now access to all the matrices `K.F`, `K.H`, `K.x`, `K.Q`, `K.R`, `K.P`... You can modify them, but be careful that inloop modifications of `K.x` or `K.P` might lead to unconsistent results! If you want to access to the `K.x` estimate, it is better to use the method 57 | ```cpp 58 | BLA::Matrix my_x = K.getxcopy(); 59 | ``` 60 | 61 | And of course you can update your Kalman filter with a new measure 62 | ```cpp 63 | obs = fill_with_sensor_measures(); // grab here your sensor data and fill in the obs vector 64 | K.update(obs); 65 | ``` -------------------------------------------------------------------------------- /examples/kalman_step/kalman_step_pyqtgraph.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Fri Aug 23 16:23:03 2019 4 | 5 | This code works with 'kalman_step.ino' 6 | > Connect your arduino to Serial port 7 | > Upload the 'kalman_step.ino' to your arduino 8 | > Eventually modify the port name "COM7" below to match yours 9 | > Run this code while your arduino transmits data 10 | 11 | To run this correctly you need the Python libraries Serial, Numpy and PyQtGraph 12 | 13 | PyQtGraph allows for real time viewing. 14 | If not installed, please use the '_matplotlib.py' code 15 | 16 | @author: rfetick 17 | """ 18 | 19 | from serial import Serial 20 | import pyqtgraph as pg 21 | from pyqtgraph.Qt import QtGui, QtCore 22 | import numpy as np 23 | import re 24 | 25 | port = "COM7" 26 | baudrate = 57600 27 | 28 | Nstate = 2 29 | Nobs = 2 30 | 31 | N = 500 # number of plotted measurements 32 | M = 4000 # number of total measurements before stop 33 | T = np.arange(N,dtype=float) 34 | 35 | STATE = np.zeros((N,Nstate)) 36 | OBS = np.zeros((N,Nobs)) 37 | KAL = np.zeros((N,Nstate)) 38 | 39 | 40 | app = QtGui.QApplication([]) 41 | 42 | win = pg.GraphicsWindow(title="ARDUINO Kalman filter") 43 | win.resize(1000,600) 44 | win.setWindowTitle('ARDUINO Kalman filter') 45 | 46 | p = win.addPlot(title="True (R), Data (G), Kalman (B)") 47 | p.setLabel('left', "State_1") 48 | p.setLabel('bottom', "Time", units='iteration') 49 | p.setYRange(-.5,1.5) 50 | p.showGrid(x=True, y=True) 51 | 52 | c1 = p.plot(STATE[:,0], pen=pg.mkPen((250,0,0),width=6), name="True") 53 | c2 = p.plot(OBS[:,0], pen=pg.mkPen((0,200,0),width=2,alpha=0.6), name="Data") 54 | c3 = p.plot(KAL[:,0], pen=pg.mkPen((50,50,250),width=4), name="Kalman") 55 | 56 | p2 = win.addPlot(title="True (R), Data (G), Kalman (B)") 57 | p2.setLabel('left', "State_2") 58 | p2.setLabel('bottom', "Time", units='iteration') 59 | p2.setYRange(-.5,1.5) 60 | p2.showGrid(x=True, y=True) 61 | 62 | c21 = p2.plot(STATE[:,1], pen=pg.mkPen((250,0,0),width=6), name="True") 63 | c22 = p2.plot(OBS[:,1], pen=pg.mkPen((0,200,0),width=2,alpha=0.6), name="Data") 64 | c23 = p2.plot(KAL[:,1], pen=pg.mkPen((50,50,250),width=4), name="Kalman") 65 | 66 | with Serial(port=port, baudrate=baudrate, timeout=1, writeTimeout=1) as port_serie: 67 | if port_serie.isOpen(): 68 | port_serie.flush() 69 | for i in range(M): 70 | ligne = port_serie.readline() 71 | j = i * (i=N) 72 | if i>=N: 73 | T = np.roll(T,-1) 74 | STATE = np.roll(STATE,-1,axis=0) 75 | OBS = np.roll(OBS,-1,axis=0) 76 | KAL = np.roll(KAL,-1,axis=0) 77 | try: 78 | temp = str(ligne)[2:-5] 79 | temp = re.findall('[-.0-9]+',temp) 80 | temp = [float(t) for t in temp] 81 | #print(temp) 82 | STATE[j,:] = temp[0:Nstate] 83 | OBS[j,:] = temp[Nstate:Nstate+Nobs] 84 | KAL[j,:] = temp[Nstate+Nobs:] 85 | 86 | c1.setData(STATE[:,0]) 87 | c2.setData(OBS[:,0]) 88 | c3.setData(KAL[:,0]) 89 | 90 | c21.setData(STATE[:,1]) 91 | c22.setData(OBS[:,1]) 92 | c23.setData(KAL[:,1]) 93 | 94 | pg.QtGui.QApplication.processEvents() 95 | 96 | except: 97 | print("Exception: %s"%ligne) 98 | port_serie.close() 99 | -------------------------------------------------------------------------------- /examples/kalman_step/kalman_step.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * Run an example of Kalman filter. 3 | * This example simulates an oscillating step-like state. 4 | * Noisy measurements are simulated with the 'SIMULATOR_' functions. 5 | * Play around with 'm1' and 'm2' gains. 6 | * Results are printed on Serial port. You can use 'kalman_step.py' to analyse them with Python. 7 | * 8 | * Author: 9 | * R.JL. Fétick 10 | * 11 | * Revision: 12 | * 24 Aug 2019 - Creation 13 | * 14 | */ 15 | 16 | #include 17 | using namespace BLA; 18 | 19 | //------------------------------------ 20 | /**** MODELIZATION PARAMETERS ****/ 21 | //------------------------------------ 22 | 23 | // Dimensions of the matrices 24 | #define Nstate 2 // length of the state vector 25 | #define Nobs 2 // length of the measurement vector 26 | 27 | // measurement std (to be characterized from your sensors) 28 | #define n1 0.2 // noise on the 1st measurement component 29 | #define n2 0.1 // noise on the 2nd measurement component 30 | 31 | // model std (1/inertia) 32 | #define m1 0.01 33 | #define m2 0.05 34 | 35 | BLA::Matrix obs; // observation vector 36 | KALMAN K; // your Kalman filter 37 | 38 | // Note: I made 'obs' a global variable so memory is allocated before the loop. 39 | // This might provide slightly better speed efficiency in loop. 40 | 41 | 42 | //------------------------------------ 43 | /**** SIMULATOR PARAMETERS ****/ 44 | //------------------------------------ 45 | 46 | BLA::Matrix state; // true state vector for simulation 47 | 48 | #define SIMUL_FREQ 250 // oscillating frequency 49 | int SIMUL_STEP = 0; // step counter 50 | 51 | //------------------------------------ 52 | /**** SETUP & LOOP ****/ 53 | //------------------------------------ 54 | 55 | void setup() { 56 | 57 | Serial.begin(57600); 58 | 59 | // The model below is very simple since matrices are diagonal! 60 | 61 | // time evolution matrix 62 | K.F = {1.0, 0.0, 63 | 0.0, 1.0}; 64 | // measurement matrix 65 | K.H = {1.0, 0.0, 66 | 0.0, 1.0}; 67 | // measurement covariance matrix 68 | K.R = {n1*n1, 0.0, 69 | 0.0, n2*n2}; 70 | // model covariance matrix 71 | K.Q = {m1*m1, 0.0, 72 | 0.0, m2*m2}; 73 | 74 | // INITIALIZE SIMULATION 75 | SIMULATOR_INIT(); 76 | 77 | } 78 | 79 | void loop() { 80 | 81 | // UPDATE THE SIMULATED PHYSICAL PROCESS 82 | SIMULATOR_UPDATE(); 83 | 84 | // SIMULATE NOISY MEASUREMENT 85 | // Result of the measurement is written into 'obs' 86 | SIMULATOR_MEASURE(); 87 | 88 | // APPLY KALMAN FILTER 89 | K.update(obs); 90 | 91 | // PRINT RESULTS: true state, measurement, estimated state 92 | Serial << state << ' ' << obs << ' ' << K.x << '\n'; 93 | } 94 | 95 | //------------------------------------ 96 | /**** SIMULATOR FUNCTIONS ****/ 97 | //------------------------------------ 98 | 99 | void SIMULATOR_INIT(){ 100 | randomSeed(analogRead(0)); 101 | state(0) = 0.0; 102 | state(1) = 0.0; 103 | } 104 | 105 | void SIMULATOR_UPDATE(){ 106 | // Simulate a physical process 107 | BLA::Matrix state_var; // state variations from the model 108 | state_var(0) = 0.0; 109 | state_var(1) = 0.0; 110 | // Step-like function 111 | if(SIMUL_STEP==0){ 112 | state_var(0) = 1.0; 113 | state_var(1) = 1.0; 114 | } 115 | if(SIMUL_STEP==SIMUL_FREQ/2){ 116 | state_var(0) = -1.0; 117 | state_var(1) = -1.0; 118 | } 119 | 120 | state = K.F * state + state_var; // time evolution 121 | 122 | SIMUL_STEP += 1; 123 | SIMUL_STEP = SIMUL_STEP % SIMUL_FREQ; 124 | } 125 | 126 | void SIMULATOR_MEASURE(){ 127 | // Simulate a noisy measurement of the physical process 128 | BLA::Matrix noise; 129 | noise(0) = n1 * SIMULATOR_GAUSS_NOISE(); 130 | noise(1) = n2 * SIMULATOR_GAUSS_NOISE(); 131 | obs = K.H * state + noise; // measurement 132 | } 133 | 134 | double SIMULATOR_GAUSS_NOISE(){ 135 | // Generate centered reduced Gaussian random number with Box-Muller algorithm 136 | double u1 = random(1,10000)/10000.0; 137 | double u2 = random(1,10000)/10000.0; 138 | return sqrt(-2*log(u1))*cos(2*M_PI*u2); 139 | } 140 | -------------------------------------------------------------------------------- /examples/kalman_full/kalman_full.ino: -------------------------------------------------------------------------------- 1 | /* 2 | * Run an example of Kalman filter. 3 | * This example simulates a sinusoidal position of an object. 4 | * The 'SIMULATOR_' functions below simulate the physical process and its measurement with sensors. 5 | * Results are printed on Serial port. You can use 'kalman_full.py' to analyse them with Python. 6 | * 7 | * Author: 8 | * R.JL. Fétick 9 | * 10 | * Revision: 11 | * 31 Aug 2019 - Creation 12 | * 13 | */ 14 | 15 | #include 16 | using namespace BLA; 17 | 18 | //------------------------------------ 19 | /**** MODELIZATION PARAMETERS ****/ 20 | //------------------------------------ 21 | 22 | #define Nstate 3 // position, speed, acceleration 23 | #define Nobs 2 // position, acceleration 24 | 25 | // measurement std of the noise 26 | #define n_p 0.3 // position measurement noise 27 | #define n_a 5.0 // acceleration measurement noise 28 | 29 | // model std (1/inertia) 30 | #define m_p 0.1 31 | #define m_s 0.1 32 | #define m_a 0.8 33 | 34 | BLA::Matrix obs; // observation vector 35 | KALMAN K; // your Kalman filter 36 | unsigned long T; // current time 37 | float DT; // delay between two updates of the filter 38 | 39 | // Note: I made 'obs' a global variable so memory is allocated before the loop. 40 | // This might provide slightly better speed efficiency in loop. 41 | 42 | 43 | //------------------------------------ 44 | /**** SIMULATOR PARAMETERS ****/ 45 | //------------------------------------ 46 | 47 | // These variables simulate a physical process to be measured 48 | // In real life, the SIMULATOR is replaced by your operational system 49 | 50 | BLA::Matrix state; // true state vector 51 | 52 | #define SIMUL_PERIOD 0.3 // oscillating period [s] 53 | #define SIMUL_AMP 1.0 // oscillation amplitude 54 | #define LOOP_DELAY 10 // add delay in the measurement loop [ms] 55 | 56 | //------------------------------------ 57 | /**** SETUP & LOOP ****/ 58 | //------------------------------------ 59 | 60 | void setup() { 61 | 62 | Serial.begin(57600); 63 | 64 | // time evolution matrix (whatever... it will be updated inloop) 65 | K.F = {1.0, 0.0, 0.0, 66 | 0.0, 1.0, 0.0, 67 | 0.0, 0.0, 1.0}; 68 | 69 | // measurement matrix n the position (e.g. GPS) and acceleration (e.g. accelerometer) 70 | K.H = {1.0, 0.0, 0.0, 71 | 0.0, 0.0, 1.0}; 72 | // measurement covariance matrix 73 | K.R = {n_p*n_p, 0.0, 74 | 0.0, n_a*n_a}; 75 | // model covariance matrix 76 | K.Q = {m_p*m_p, 0.0, 0.0, 77 | 0.0, m_s*m_s, 0.0, 78 | 0.0, 0.0, m_a*m_a}; 79 | 80 | T = millis(); 81 | 82 | // INITIALIZE SIMULATION 83 | SIMULATOR_INIT(); 84 | 85 | } 86 | 87 | void loop() { 88 | 89 | // TIME COMPUTATION 90 | DT = (millis()-T)/1000.0; 91 | T = millis(); 92 | 93 | // UPDATE STATE EQUATION 94 | // Here we make use of the Taylor expansion on the (position,speed,acceleration) 95 | // position_{k+1} = position_{k} + DT*speed_{k} + (DT*DT/2)*acceleration_{k} 96 | // speed_{k+1} = speed_{k} + DT*acceleration_{k} 97 | // acceleration_{k+1} = acceleration_{k} 98 | K.F = {1.0, DT, DT*DT/2, 99 | 0.0, 1.0, DT, 100 | 0.0, 0.0, 1.0}; 101 | 102 | // UPDATE THE SIMULATED PHYSICAL PROCESS 103 | SIMULATOR_UPDATE(); 104 | 105 | // SIMULATE A NOISY MEASUREMENT WITH A SENSOR 106 | // Result of the measurement is written into 'obs' 107 | SIMULATOR_MEASURE(); 108 | 109 | // APPLY KALMAN FILTER 110 | K.update(obs); 111 | 112 | // PRINT RESULTS: true state, measurements, estimated state, posterior covariance 113 | // The most important variable for you might be the estimated state 'K.x' 114 | Serial << state << ' ' << obs << ' ' << K.x << ' ' << K.P << '\n'; 115 | } 116 | 117 | //------------------------------------ 118 | /**** SIMULATOR FUNCTIONS ****/ 119 | //------------------------------------ 120 | 121 | void SIMULATOR_INIT(){ 122 | // Initialize stuff for the simulator 123 | randomSeed(analogRead(0)); 124 | state.Fill(0.0); 125 | obs.Fill(0.0); 126 | } 127 | 128 | void SIMULATOR_UPDATE(){ 129 | // Simulate a physical process 130 | // Here we simulate a sinusoidal position of an object 131 | unsigned long tcur = millis(); 132 | state(0) = SIMUL_AMP*sin(tcur/1000.0/SIMUL_PERIOD); // position 133 | state(1) = SIMUL_AMP/SIMUL_PERIOD*cos(tcur/1000.0/SIMUL_PERIOD); // speed 134 | state(2) = -SIMUL_AMP/SIMUL_PERIOD/SIMUL_PERIOD*sin(tcur/1000.0/SIMUL_PERIOD); // acceleration 135 | } 136 | 137 | void SIMULATOR_MEASURE(){ 138 | // Simulate a noisy measurement of the physical process 139 | BLA::Matrix noise; 140 | noise(0) = n_p * SIMULATOR_GAUSS_NOISE(); 141 | noise(1) = n_a * SIMULATOR_GAUSS_NOISE(); 142 | obs = K.H * state + noise; // measurement 143 | delay(LOOP_DELAY); //simulate a delay in the measurement 144 | } 145 | 146 | double SIMULATOR_GAUSS_NOISE(){ 147 | // Generate centered reduced Gaussian random number with Box-Muller algorithm 148 | double u1 = random(1,10000)/10000.0; 149 | double u2 = random(1,10000)/10000.0; 150 | return sqrt(-2*log(u1))*cos(2*M_PI*u2); 151 | } -------------------------------------------------------------------------------- /Kalman.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Implement the Kalman filter corresponding to the linear problem 3 | * x_k = F*x_{k-1} + B*u_k + q_k (evolution model) 4 | * y_k = H*x_k + r_k (measure) 5 | * 6 | * with the matrices and vectors 7 | * x [output] [size=Nstate] Estimated state vector 8 | * F [input] [size=(Nstate,Nstate)] Free evolution of the state vector 9 | * B [input] [size=(Nstate,Ncom)] [optional] Command vector acting on state 10 | * Q [input] [size=(Nstate,Nstate)] Model covariance acting as (1/inertia) 11 | * y [input] [size=Nobs] Observed (measured) data from sensors 12 | * H [input] [size=(Nobs,Nstate)] Observation matrix 13 | * R [input] [size=(Nobs,Nobs)] Measurement noise covariance matrix 14 | * 15 | * Many attributes are public, so you might modify them as you wish. 16 | * However be careful since modification of attributes (especially 'P' and 'x') 17 | * might lead to unconsistant results. 18 | * Use the 'getxcopy' method to get a copy of the 'x' state vector. 19 | * 20 | * Requires: 21 | * BasicLinearAlgebra https://github.com/tomstewart89/BasicLinearAlgebra 22 | * 23 | * License: 24 | * See the LICENSE file 25 | * 26 | * Author: 27 | * R.JL. Fétick 28 | * 29 | */ 30 | 31 | #ifndef Kalman_h 32 | #define Kalman_h 33 | 34 | #include 35 | 36 | #include 37 | 38 | #define KALMAN_CHECK true 39 | #define KALMAN_VERBOSE false 40 | 41 | using namespace BLA; 42 | 43 | /********** PARTICULAR MATRIX DEFINITION **********/ 44 | // These matrices require less memory than normal ones. 45 | // If your matrices are symmetric or triangular, use these ones for Arduino SRAM saving 46 | 47 | // Symmetric (i=row*cols+col) 48 | template struct Symmetric{ 49 | mutable ElemT m[dim*(dim+1)/2]; 50 | typedef ElemT elem_t; 51 | ElemT &operator()(int row, int col) const{ 52 | static ElemT dummy; 53 | if(col < dim && row < dim){ 54 | if(col < row){ // swap row and col 55 | ElemT temp = row; 56 | row = col; 57 | col = temp; 58 | } 59 | return m[(2*dim-row+1)*row/2+col-row]; 60 | }else 61 | return (dummy = 0); 62 | } 63 | }; 64 | 65 | // SkewSymmetric (i=row*cols+col) 66 | template struct SkewSymmetric{ 67 | mutable ElemT m[dim*(dim+1)/2]; 68 | typedef ElemT elem_t; 69 | ElemT &operator()(int row, int col) const{ 70 | static ElemT dummy; 71 | if(col < dim && row < dim && row!=col){ 72 | if(col < row){ 73 | return -m[(2*dim-col+1)*col/2+row-col]; 74 | }else{ 75 | return m[(2*dim-row+1)*row/2+col-row]; 76 | } 77 | }else{ 78 | return (dummy = 0); 79 | } 80 | } 81 | }; 82 | 83 | // Triangular sup (i=row*cols+col) 84 | template struct TriangularSup{ 85 | mutable ElemT m[dim*(dim+1)/2]; 86 | typedef ElemT elem_t; 87 | ElemT &operator()(int row, int col) const{ 88 | static ElemT dummy; 89 | if(col < dim && row < dim && col>=row){ 90 | return m[(2*dim-row+1)*row/2+col-row]; 91 | }else 92 | return (dummy = 0); 93 | } 94 | }; 95 | 96 | // Triangular inf (i=row*cols+col) 97 | template struct TriangularInf{ 98 | mutable ElemT m[dim*(dim+1)/2]; 99 | typedef ElemT elem_t; 100 | ElemT &operator()(int row, int col) const{ 101 | static ElemT dummy; 102 | if(col < dim && row < dim && row>=col){ 103 | return m[(2*dim-col+1)*col/2+row-col]; 104 | }else 105 | return (dummy = 0); 106 | } 107 | }; 108 | 109 | // Diagonal template comes from Tom Stewart (BasicLinearAlgebra) 110 | template struct Diagonal{ 111 | mutable ElemT m[dim]; 112 | // The only requirement on this class is that it implement the () operator like so: 113 | typedef ElemT elem_t; 114 | 115 | ElemT &operator()(int row, int col) const 116 | { 117 | static ElemT dummy; 118 | // If it's on the diagonal and it's not larger than the matrix dimensions then return the element 119 | if(row == col && row < dim) 120 | return m[row]; 121 | else 122 | return (dummy = 0); 123 | } 124 | }; 125 | 126 | 127 | 128 | /********** CLASS DEFINITION **********/ 129 | 130 | // Last arg of template allows to eventually define Symmetric, AntiSymmetric, TriangularSup or TriangularInf matrices for memory saving 131 | template > 132 | class KALMAN{ 133 | private: 134 | void _update(BLA::Matrix obs, BLA::Matrix comstate); 135 | BLA::Identity Id; // Identity matrix 136 | public: 137 | //INPUT MATRICES 138 | BLA::Matrix F; // time evolution matrix 139 | BLA::Matrix H; // observation matrix 140 | BLA::Matrix B; // Command matrix (optional) 141 | BLA::Matrix > Q; // model noise covariance matrix 142 | BLA::Matrix > R; // measure noise covariance matrix 143 | //OUTPUT MATRICES 144 | BLA::Matrix > P; // posterior covariance (do not modify, except to init!) 145 | BLA::Matrix x; // state vector (do not modify, except to init!) 146 | 147 | int status; // 0 if Kalman filter computed correctly 148 | 149 | // UPDATE FILTER WITH OBSERVATION 150 | void update(BLA::Matrix obs); 151 | 152 | // UPDATE FILTER WITH OBSERVATION and COMMAND 153 | void update(BLA::Matrix obs, BLA::Matrix com); 154 | 155 | // CONSTRUCTOR 156 | KALMAN(); 157 | 158 | // GETTER on X (copy vector to avoid eventual user modifications) 159 | BLA::Matrix getxcopy(); 160 | 161 | }; 162 | 163 | /********** PRIVATE IMPLEMENTATION of UPDATE **********/ 164 | 165 | template 166 | void KALMAN::_update(BLA::Matrix obs, BLA::Matrix comstate){ 167 | if(KALMAN_CHECK){ 168 | for(int i=0;i S; 177 | BLA::Matrix K; // Kalman gain matrix 178 | // UPDATE 179 | this->x = this->F * this->x + comstate; 180 | this->P = this->F * this->P * (~ this->F) + this->Q; 181 | // ESTIMATION 182 | S = this->H * this->P * (~ this->H) + this->R; 183 | bool is_nonsingular = Invert(S); // inverse inplace (S <- S^{-1}) 184 | K = P*(~H)*S; 185 | if(is_nonsingular){ 186 | this->x += K*(obs - this->H * this->x); // K*y 187 | this->P = (this->Id - K * this->H)* this->P; 188 | if(KALMAN_CHECK){ 189 | for(int i=0;ix(i)) || isinf(this->x(i))){ 191 | if(KALMAN_VERBOSE){Serial.println(F("KALMAN:ERROR: estimated vector has nan or inf values"));} 192 | status = 1; 193 | return; 194 | } 195 | } 196 | } 197 | }else{ 198 | if(KALMAN_VERBOSE){Serial.println(F("KALMAN:ERROR: could not invert S matrix. Try to reset P matrix."));} 199 | this->P.Fill(0.0); // try to reset P. Better strategy? 200 | //K.Fill(0.0); 201 | } 202 | }; 203 | 204 | 205 | /********** UPDATE with OBS & COM **********/ 206 | template 207 | void KALMAN::update(BLA::Matrix obs, BLA::Matrix com){ 208 | if(KALMAN_CHECK){ 209 | for(int i=0;iB *com); 218 | }; 219 | 220 | /********** UPDATE with OBS **********/ 221 | 222 | template 223 | void KALMAN::update(BLA::Matrix obs){ 224 | BLA::Zeros NULLCOMSTATE; 225 | _update(obs,NULLCOMSTATE); 226 | }; 227 | 228 | /********** CONSTRUCTOR **********/ 229 | 230 | template 231 | KALMAN::KALMAN(){ 232 | if(KALMAN_VERBOSE){ 233 | Serial.println(F("KALMAN:INFO: Initialize filter")); 234 | if((Nstate<=1)||(Nobs<=1)){ 235 | Serial.println(F("KALMAN:ERROR: 'Nstate' and 'Nobs' must be > 1")); 236 | } 237 | } 238 | this->P.Fill(0.0); 239 | this->x.Fill(0.0); 240 | }; 241 | 242 | /********** GETXCOPY **********/ 243 | 244 | template 245 | BLA::Matrix KALMAN::getxcopy(){ 246 | BLA::Matrix out; 247 | for(int i=0;ix(i); 249 | } 250 | return out; 251 | }; 252 | 253 | 254 | #endif 255 | --------------------------------------------------------------------------------