├── README.md ├── kalman.m ├── fusion.m └── LICENSE.md /README.md: -------------------------------------------------------------------------------- 1 | SensorFusion 2 | ============ 3 | 4 | A simple Matlab example of sensor fusion using a Kalman filter. 5 | 6 | To run, just launch Matlab, change your directory to where you put the repository, and do 7 | 8 | >> fusion 9 | 10 | See this [tutorial](https://simondlevy.academic.wlu.edu/kalman-tutorial/) for a complete discussion 11 | -------------------------------------------------------------------------------- /kalman.m: -------------------------------------------------------------------------------- 1 | function xhat = kalman(z, A, C, R, Q) 2 | % Simple Kalman Filter (linear) with optimal gain, no control signal 3 | % 4 | % z Measurement signal m observations X # of observations 5 | % A State transition model n X n, n = # of state values 6 | % C Observation model m X n 7 | % R Covariance of observation noise m X m 8 | % Q Covariance of process noise n X n 9 | % 10 | % Based on http://en.wikipedia.org/wiki/Kalman_filter, but matrices named 11 | % A, C, G instead of F, H, K. 12 | % 13 | % See http://home.wlu.edu/~levys/kalman_tutorial for background 14 | % 15 | % Copyright (C) 2014 Simon D. Levy 16 | % 17 | % This code is free software: you can redistribute it and/or modify 18 | % it under the terms of the GNU Lesser General Public License as 19 | % published by the Free Software Foundation, either version 3 of the 20 | % License, or (at your option) any later version. 21 | % 22 | % This code is distributed in the hope that it will be useful, 23 | % but WITHOUT ANY WARRANTY without even the implied warranty of 24 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 25 | % GNU General Public License for more details. 26 | % 27 | % You should have received a copy of the GNU Lesser General Public License 28 | % along with this code. If not, see . 29 | 30 | % Initializtion ===================================================== 31 | 32 | % Number of sensors 33 | m = size(C, 1); 34 | 35 | % Number of state values 36 | n = size(C, 2); 37 | 38 | % Number of observations 39 | numobs = size(z, 2); 40 | 41 | % Use linear least squares to estimate initial state from initial 42 | % observation 43 | xhat = zeros(n, numobs); 44 | xhat(:,1) = C \ z(:,1); 45 | 46 | % Initialize P, I 47 | P = ones(size(A)); 48 | I = eye(size(A)); 49 | 50 | % Kalman Filter ===================================================== 51 | 52 | for k = 2:numobs 53 | 54 | % Predict 55 | xhat(:,k) = A * xhat(:,k-1); 56 | P = A * P * A' + Q; 57 | 58 | % Update 59 | G = P * C' / (C * P * C' + R); 60 | P = (I - G * C) * P; 61 | xhat(:,k) = xhat(:,k) + G * (z(:,k) - C * xhat(:,k)); 62 | 63 | end 64 | -------------------------------------------------------------------------------- /fusion.m: -------------------------------------------------------------------------------- 1 | function fusion 2 | % Kalman Filter sensor fusion example based on 3 | % 4 | % http://www.slideshare.net/antoniomorancardenas/data-fusion-with-kalman-filtering 5 | % 6 | % % See http://home.wlu.edu/~levys/kalman_tutorial for background 7 | % 8 | % Copyright (C) 2014 Simon D. Levy 9 | % 10 | % This code is free software: you can redistribute it and/or modify 11 | % it under the terms of the GNU Lesser General Public License as 12 | % published by the Free Software Foundation, either version 3 of the 13 | % License, or (at your option) any later version. 14 | % 15 | % This code is distributed in the hope that it will be useful, 16 | % but WITHOUT ANY WARRANTY without even the implied warranty of 17 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | % GNU General Public License for more details. 19 | % 20 | % You should have received a copy of the GNU Lesser General Public License 21 | % along with this code. If not, see . 22 | 23 | % Parameters ============================================================ 24 | 25 | % Biases 26 | BIAS1 = +1; 27 | BIAS2 = -1; 28 | 29 | % Measurement noise covariances 30 | R1 = 0.64; 31 | R2 = 0.64; 32 | 33 | % Process noise covariance 34 | Q = .005; 35 | 36 | % State transition model 37 | A = 1; 38 | 39 | % Observation model 40 | C1 = 1; 41 | C2 = 1; 42 | 43 | % Duration 44 | N = 1000; 45 | 46 | % Run ==================================================================== 47 | 48 | % Generate the signal x as a sine wave 49 | x = 20 + sin(5*linspace(0,1,N)*pi); 50 | 51 | % Add some process noise with covariance Q 52 | w = sqrt(Q) * randn(size(x)); 53 | x = x + w; 54 | 55 | % Compute noisy sensor values 56 | z1 = BIAS1 + x + sqrt(R1) * randn(size(x)); 57 | z2 = BIAS2 + x + sqrt(R2) * randn(size(x)); 58 | 59 | % Run a single-sensor example and plot it 60 | xhat = kalman(z1, A, C1, R1, Q); 61 | 62 | % Plot sensed and estimated values 63 | clf 64 | plotsigs(1, z1, xhat, 'Sensor 1') 65 | title('One sensor: signal cleanup') 66 | 67 | % Plot estimate and actual values 68 | plotsigsrms('One sensor', 2, x, xhat) 69 | 70 | % Run the Kalman filter on fused sensors 71 | xhat = kalman([z1; z2], A, [C1; C2], [R1 0; 0 R2], Q); 72 | 73 | % Plot fusion example 74 | plotsigsrms('Two sensors', 3, x, xhat) 75 | 76 | 77 | % Helper functions ====================================================== 78 | 79 | function plotsigs(pos, sig1, sig2, sig1label) 80 | subplot(3,1,pos) 81 | hold on 82 | plot(sig1, 'k') 83 | plot(sig2, 'r') 84 | legend({sig1label, 'Estimated'}) 85 | ylim([15 25]) 86 | hold off 87 | 88 | function plotsigsrms(label, pos, x, xhat) 89 | plotsigs(pos, x, xhat, 'Actual') 90 | title(sprintf('%s: RMS error = %f', label, sqrt(sum((x-xhat).^2)/length(x)))) 91 | 92 | 93 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | --------------------------------------------------------------------------------