├── 05-EKFSLAM ├── legend5.png ├── EKFSLAM05.fig ├── landmarks.mat ├── waypoints.mat ├── landmarks2.mat ├── waypoints2.mat └── EKFSLAM05.m ├── 01-KalmanFilter ├── legend1.png ├── KalmanF01.fig └── KalmanF01.m ├── 02-KalmanFilter ├── legend2.png ├── KalmanF02.fig └── KalmanF02.m ├── 03-KalmanFilter ├── legend3.png ├── KalmanF03.fig └── KalmanF03.m ├── 04-ParticleFilter ├── legend4.png ├── ParticleF04.fig └── ParticleF04.m ├── README.md └── LICENSE /05-EKFSLAM/legend5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/05-EKFSLAM/legend5.png -------------------------------------------------------------------------------- /05-EKFSLAM/EKFSLAM05.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/05-EKFSLAM/EKFSLAM05.fig -------------------------------------------------------------------------------- /05-EKFSLAM/landmarks.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/05-EKFSLAM/landmarks.mat -------------------------------------------------------------------------------- /05-EKFSLAM/waypoints.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/05-EKFSLAM/waypoints.mat -------------------------------------------------------------------------------- /01-KalmanFilter/legend1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/01-KalmanFilter/legend1.png -------------------------------------------------------------------------------- /02-KalmanFilter/legend2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/02-KalmanFilter/legend2.png -------------------------------------------------------------------------------- /03-KalmanFilter/legend3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/03-KalmanFilter/legend3.png -------------------------------------------------------------------------------- /05-EKFSLAM/landmarks2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/05-EKFSLAM/landmarks2.mat -------------------------------------------------------------------------------- /05-EKFSLAM/waypoints2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/05-EKFSLAM/waypoints2.mat -------------------------------------------------------------------------------- /01-KalmanFilter/KalmanF01.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/01-KalmanFilter/KalmanF01.fig -------------------------------------------------------------------------------- /02-KalmanFilter/KalmanF02.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/02-KalmanFilter/KalmanF02.fig -------------------------------------------------------------------------------- /03-KalmanFilter/KalmanF03.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/03-KalmanFilter/KalmanF03.fig -------------------------------------------------------------------------------- /04-ParticleFilter/legend4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/04-ParticleFilter/legend4.png -------------------------------------------------------------------------------- /04-ParticleFilter/ParticleF04.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/slabua/robotics-filters/HEAD/04-ParticleFilter/ParticleF04.fig -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # robotics-filters 2 | [![License: GPLv3][GPLimg]][GPLurl] 3 | 4 | Copyright (C) 2013 5 | Salvatore La Bua [slabua(at)gmail.com](mailto:slabua@gmail.com) 6 | MTM 7 | GLB 8 | 9 | DICGIM - University of Palermo 10 | 11 | 12 | 13 | ## Table of Contents 14 | 15 | - [Introduction to the Project](#introduction-to-the-project) 16 | - [Filters](#filters) 17 | - [KalmanF01 (Linear Kalman Filter)](#kalmanf01-linear-kalman-filter) 18 | - [KalmanF02 (Extended Kalman Filter)](#kalmanf02-extended-kalman-filter) 19 | - [KalmanF03 (Extended Kalman Filter)](#kalmanf03-extended-kalman-filter) 20 | - [ParticleF04](#particlef04) 21 | - [EKFSLAM05](#ekfslam05) 22 | - [LICENSE](#license) 23 | - [Screenshots](#screenshots) 24 | - [KalmanF01](#kalmanf01) 25 | - [KalmanF02](#kalmanf02) 26 | - [KalmanF03](#kalmanf03) 27 | - [ParticleF04](#particlef04-1) 28 | - [EKFSLAM05](#ekfslam05-1) 29 | 30 | 31 | ## Introduction to the Project 32 | 33 | Kalman, Particle and SLAM Filters implemented for the 2012/2013 Robotics course. 34 | 35 | ## Filters 36 | 37 | ### KalmanF01 (Linear Kalman Filter) 38 | The 2D robot (x, y) follows a linear path using the distance to a wall to localise itself. 39 | 40 | ### KalmanF02 (Extended Kalman Filter) 41 | The 2D robot (x, y) follows a linear path using the distance to a landmark to localise itself. 42 | 43 | ### KalmanF03 (Extended Kalman Filter) 44 | The 3D robot (x, y, θ) follows a linear path using the distance to a wall to localise itself. 45 | 46 | ### ParticleF04 47 | The 3D robot (x, y, θ) follows a linear path using the distance to the closest landmark to localise itself. 48 | 49 | ### EKFSLAM05 50 | The 3D robot (x, y, θ) follows a path defined by a series of waypoints using the distance to the landmarks to localise itself and internally build a map of the environment. 51 | 52 | ## LICENSE 53 | 54 | This program is free software: you can redistribute it and/or modify 55 | it under the terms of the GNU General Public License as published by 56 | the Free Software Foundation, either version 3 of the License, or 57 | (at your option) any later version. 58 | 59 | This program is distributed in the hope that it will be useful, 60 | but WITHOUT ANY WARRANTY; without even the implied warranty of 61 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 62 | GNU General Public License for more details. 63 | 64 | You should have received a copy of the GNU General Public License 65 | along with this program. If not, see . 66 | 67 | ## Screenshots 68 | 69 | ### KalmanF01 70 | ![KalmanF01][S01] 71 | 72 | ### KalmanF02 73 | ![KalmanF02][S02] 74 | 75 | ### KalmanF03 76 | ![KalmanF03][S03] 77 | 78 | ### ParticleF04 79 | ![ParticleF04][S04] 80 | 81 | ### EKFSLAM05 82 | ![EKFSLAM05][S05] 83 | 84 | [GPLimg]: https://img.shields.io/badge/License-GPLv3-blue.svg 85 | [GPLurl]: https://www.gnu.org/licenses/gpl-3.0 86 | [S01]: https://goo.gl/mpN1rH 87 | [S02]: https://goo.gl/NNciBb 88 | [S03]: https://goo.gl/BbF4zo 89 | [S04]: https://goo.gl/MSDfxo 90 | [S05]: https://goo.gl/p8htFK 91 | 92 | -------------------------------------------------------------------------------- /02-KalmanFilter/KalmanF02.m: -------------------------------------------------------------------------------- 1 | function varargout = KalmanF02(varargin) 2 | % KALMANF02 MATLAB code for KalmanF02.fig 3 | % KALMANF02, by itself, creates a new KALMANF02 or raises the existing 4 | % singleton*. 5 | % 6 | % H = KALMANF02 returns the handle to a new KALMANF02 or the handle to 7 | % the existing singleton*. 8 | % 9 | % KALMANF02('CALLBACK', hObject, eventData, handles, ...) calls the local 10 | % function named CALLBACK in KALMANF02.M with the given input arguments. 11 | % 12 | % KALMANF02('Property', 'Value', ...) creates a new KALMANF02 or raises the 13 | % existing singleton*. Starting from the left, property value pairs are 14 | % applied to the GUI before KalmanF02_OpeningFcn gets called. An 15 | % unrecognized property name or invalid value makes property application 16 | % stop. All inputs are passed to KalmanF02_OpeningFcn via varargin. 17 | % 18 | % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one 19 | % instance to run (singleton)". 20 | % 21 | % See also: GUIDE, GUIDATA, GUIHANDLES 22 | % 23 | % -- 24 | % Salvatore La Bua (slabua@gmail.com) 25 | % GLB 26 | % MTM 27 | % DICGIM - University of Palermo 28 | 29 | % Edit the above text to modify the response to help KalmanF02 30 | 31 | % Last Modified by GUIDE v2.5 16-Oct-2013 01:49:58 32 | 33 | % Begin initialization code - DO NOT EDIT 34 | gui_Singleton = 1; 35 | gui_State = struct('gui_Name', mfilename, ... 36 | 'gui_Singleton', gui_Singleton, ... 37 | 'gui_OpeningFcn', @KalmanF02_OpeningFcn, ... 38 | 'gui_OutputFcn', @KalmanF02_OutputFcn, ... 39 | 'gui_LayoutFcn', [], ... 40 | 'gui_Callback', []); 41 | if nargin && ischar(varargin{1}) 42 | gui_State.gui_Callback = str2func(varargin{1}); 43 | end 44 | 45 | if nargout 46 | [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); 47 | else 48 | gui_mainfcn(gui_State, varargin{:}); 49 | end 50 | % End initialization code - DO NOT EDIT 51 | 52 | % --- Executes just before KalmanF02 is made visible. 53 | function KalmanF02_OpeningFcn(hObject, eventdata, handles, varargin) 54 | % This function has no output args, see OutputFcn. 55 | % hObject handle to figure 56 | % eventdata reserved - to be defined in a future version of MATLAB 57 | % handles structure with handles and user data (see GUIDATA) 58 | % varargin command line arguments to KalmanF02 (see VARARGIN) 59 | handles = initialize_gui(hObject, handles); 60 | % Choose default command line output for KalmanF02 61 | handles.output = hObject; 62 | 63 | % Update handles structure 64 | guidata(hObject, handles); 65 | 66 | % UIWAIT makes KalmanF02 wait for user response (see UIRESUME) 67 | % uiwait(handles.figure1); 68 | 69 | % --- Outputs from this function are returned to the command line. 70 | function varargout = KalmanF02_OutputFcn(hObject, eventdata, handles) 71 | % varargout cell array for returning output args (see VARARGOUT); 72 | % hObject handle to figure 73 | % eventdata reserved - to be defined in a future version of MATLAB 74 | % handles structure with handles and user data (see GUIDATA) 75 | 76 | % Get default command line output from handles structure 77 | varargout{1} = handles.output; 78 | 79 | % --- Executes on button press in Default. 80 | function Default_Callback(hObject, eventdata, handles) 81 | % hObject handle to Default (see GCBO) 82 | % eventdata reserved - to be defined in a future version of MATLAB 83 | % handles structure with handles and user data (see GUIDATA) 84 | flag_default = get(handles.Default, 'Value'); 85 | if (flag_default) 86 | set_Default(hObject, handles); 87 | disable_fields(hObject, handles); 88 | else 89 | set_Default(hObject, handles); 90 | enable_fields(hObject, handles); 91 | end 92 | % Hint: get(hObject, 'Value') returns toggle state of Default 93 | 94 | function Mu0_X_Callback(hObject, eventdata, handles) 95 | % hObject handle to Mu0_X (see GCBO) 96 | % eventdata reserved - to be defined in a future version of MATLAB 97 | % handles structure with handles and user data (see GUIDATA) 98 | 99 | % Hints: get(hObject, 'String') returns contents of Mu0_X as text 100 | % str2double(get(hObject, 'String')) returns contents of Mu0_X as a double 101 | 102 | % --- Executes during object creation, after setting all properties. 103 | function Mu0_X_CreateFcn(hObject, eventdata, handles) 104 | % hObject handle to Mu0_X (see GCBO) 105 | % eventdata reserved - to be defined in a future version of MATLAB 106 | % handles empty - handles not created until after all CreateFcns called 107 | 108 | % Hint: edit controls usually have a white background on Windows. 109 | % See ISPC and COMPUTER. 110 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 111 | set(hObject, 'BackgroundColor', 'white'); 112 | end 113 | 114 | function Mu0_Y_Callback(hObject, eventdata, handles) 115 | % hObject handle to Mu0_Y (see GCBO) 116 | % eventdata reserved - to be defined in a future version of MATLAB 117 | % handles structure with handles and user data (see GUIDATA) 118 | 119 | % Hints: get(hObject, 'String') returns contents of Mu0_Y as text 120 | % str2double(get(hObject, 'String')) returns contents of Mu0_Y as a double 121 | 122 | % --- Executes during object creation, after setting all properties. 123 | function Mu0_Y_CreateFcn(hObject, eventdata, handles) 124 | % hObject handle to Mu0_Y (see GCBO) 125 | % eventdata reserved - to be defined in a future version of MATLAB 126 | % handles empty - handles not created until after all CreateFcns called 127 | 128 | % Hint: edit controls usually have a white background on Windows. 129 | % See ISPC and COMPUTER. 130 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 131 | set(hObject, 'BackgroundColor', 'white'); 132 | end 133 | 134 | function X0_X_Callback(hObject, eventdata, handles) 135 | % hObject handle to X0_X (see GCBO) 136 | % eventdata reserved - to be defined in a future version of MATLAB 137 | % handles structure with handles and user data (see GUIDATA) 138 | 139 | % Hints: get(hObject, 'String') returns contents of X0_X as text 140 | % str2double(get(hObject, 'String')) returns contents of X0_X as a double 141 | 142 | % --- Executes during object creation, after setting all properties. 143 | function X0_X_CreateFcn(hObject, eventdata, handles) 144 | % hObject handle to X0_X (see GCBO) 145 | % eventdata reserved - to be defined in a future version of MATLAB 146 | % handles empty - handles not created until after all CreateFcns called 147 | 148 | % Hint: edit controls usually have a white background on Windows. 149 | % See ISPC and COMPUTER. 150 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 151 | set(hObject, 'BackgroundColor', 'white'); 152 | end 153 | 154 | function X0_Y_Callback(hObject, eventdata, handles) 155 | % hObject handle to X0_Y (see GCBO) 156 | % eventdata reserved - to be defined in a future version of MATLAB 157 | % handles structure with handles and user data (see GUIDATA) 158 | 159 | % Hints: get(hObject, 'String') returns contents of X0_Y as text 160 | % str2double(get(hObject, 'String')) returns contents of X0_Y as a double 161 | 162 | % --- Executes during object creation, after setting all properties. 163 | function X0_Y_CreateFcn(hObject, eventdata, handles) 164 | % hObject handle to X0_Y (see GCBO) 165 | % eventdata reserved - to be defined in a future version of MATLAB 166 | % handles empty - handles not created until after all CreateFcns called 167 | 168 | % Hint: edit controls usually have a white background on Windows. 169 | % See ISPC and COMPUTER. 170 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 171 | set(hObject, 'BackgroundColor', 'white'); 172 | end 173 | 174 | function Direction_Callback(hObject, eventdata, handles) 175 | % hObject handle to Direction (see GCBO) 176 | % eventdata reserved - to be defined in a future version of MATLAB 177 | % handles structure with handles and user data (see GUIDATA) 178 | 179 | % Hints: get(hObject, 'String') returns contents of Direction as text 180 | % str2double(get(hObject, 'String')) returns contents of Direction as a double 181 | 182 | % --- Executes during object creation, after setting all properties. 183 | function Direction_CreateFcn(hObject, eventdata, handles) 184 | % hObject handle to Direction (see GCBO) 185 | % eventdata reserved - to be defined in a future version of MATLAB 186 | % handles empty - handles not created until after all CreateFcns called 187 | 188 | % Hint: edit controls usually have a white background on Windows. 189 | % See ISPC and COMPUTER. 190 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 191 | set(hObject, 'BackgroundColor', 'white'); 192 | end 193 | 194 | function Control_noise_Callback(hObject, eventdata, handles) 195 | % hObject handle to Control_noise (see GCBO) 196 | % eventdata reserved - to be defined in a future version of MATLAB 197 | % handles structure with handles and user data (see GUIDATA) 198 | 199 | % Hints: get(hObject, 'String') returns contents of Control_noise as text 200 | % str2double(get(hObject, 'String')) returns contents of Control_noise as a double 201 | 202 | % --- Executes during object creation, after setting all properties. 203 | function Control_noise_CreateFcn(hObject, eventdata, handles) 204 | % hObject handle to Control_noise (see GCBO) 205 | % eventdata reserved - to be defined in a future version of MATLAB 206 | % handles empty - handles not created until after all CreateFcns called 207 | 208 | % Hint: edit controls usually have a white background on Windows. 209 | % See ISPC and COMPUTER. 210 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 211 | set(hObject, 'BackgroundColor', 'white'); 212 | end 213 | 214 | function Sensor_noise_Callback(hObject, eventdata, handles) 215 | % hObject handle to Sensor_noise (see GCBO) 216 | % eventdata reserved - to be defined in a future version of MATLAB 217 | % handles structure with handles and user data (see GUIDATA) 218 | 219 | % Hints: get(hObject, 'String') returns contents of Sensor_noise as text 220 | % str2double(get(hObject, 'String')) returns contents of Sensor_noise as a double 221 | 222 | % --- Executes during object creation, after setting all properties. 223 | function Sensor_noise_CreateFcn(hObject, eventdata, handles) 224 | % hObject handle to Sensor_noise (see GCBO) 225 | % eventdata reserved - to be defined in a future version of MATLAB 226 | % handles empty - handles not created until after all CreateFcns called 227 | 228 | % Hint: edit controls usually have a white background on Windows. 229 | % See ISPC and COMPUTER. 230 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 231 | set(hObject, 'BackgroundColor', 'white'); 232 | end 233 | 234 | function Sigma_stdX_Callback(hObject, eventdata, handles) 235 | % hObject handle to Sigma_stdX (see GCBO) 236 | % eventdata reserved - to be defined in a future version of MATLAB 237 | % handles structure with handles and user data (see GUIDATA) 238 | 239 | % Hints: get(hObject, 'String') returns contents of Sigma_stdX as text 240 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdX as a double 241 | 242 | % --- Executes during object creation, after setting all properties. 243 | function Sigma_stdX_CreateFcn(hObject, eventdata, handles) 244 | % hObject handle to Sigma_stdX (see GCBO) 245 | % eventdata reserved - to be defined in a future version of MATLAB 246 | % handles empty - handles not created until after all CreateFcns called 247 | 248 | % Hint: edit controls usually have a white background on Windows. 249 | % See ISPC and COMPUTER. 250 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 251 | set(hObject, 'BackgroundColor', 'white'); 252 | end 253 | 254 | function Sigma_stdY_Callback(hObject, eventdata, handles) 255 | % hObject handle to Sigma_stdX (see GCBO) 256 | % eventdata reserved - to be defined in a future version of MATLAB 257 | % handles structure with handles and user data (see GUIDATA) 258 | 259 | % Hints: get(hObject, 'String') returns contents of Sigma_stdX as text 260 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdX as a double 261 | 262 | % --- Executes during object creation, after setting all properties. 263 | function Sigma_stdY_CreateFcn(hObject, eventdata, handles) 264 | % hObject handle to Sigma_stdX (see GCBO) 265 | % eventdata reserved - to be defined in a future version of MATLAB 266 | % handles empty - handles not created until after all CreateFcns called 267 | 268 | % Hint: edit controls usually have a white background on Windows. 269 | % See ISPC and COMPUTER. 270 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 271 | set(hObject, 'BackgroundColor', 'white'); 272 | end 273 | 274 | function Sigma_std_Callback(hObject, eventdata, handles) 275 | % hObject handle to Sigma_stdY (see GCBO) 276 | % eventdata reserved - to be defined in a future version of MATLAB 277 | % handles structure with handles and user data (see GUIDATA) 278 | 279 | % Hints: get(hObject, 'String') returns contents of Sigma_stdY as text 280 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdY as a double 281 | 282 | % --- Executes during object creation, after setting all properties. 283 | function Sigma_std_CreateFcn(hObject, eventdata, handles) 284 | % hObject handle to Sigma_stdY (see GCBO) 285 | % eventdata reserved - to be defined in a future version of MATLAB 286 | % handles empty - handles not created until after all CreateFcns called 287 | 288 | % Hint: edit controls usually have a white background on Windows. 289 | % See ISPC and COMPUTER. 290 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 291 | set(hObject, 'BackgroundColor', 'white'); 292 | end 293 | 294 | function Linear_velocity_Callback(hObject, eventdata, handles) 295 | % hObject handle to Linear_velocity (see GCBO) 296 | % eventdata reserved - to be defined in a future version of MATLAB 297 | % handles structure with handles and user data (see GUIDATA) 298 | 299 | % Hints: get(hObject, 'String') returns contents of Linear_velocity as text 300 | % str2double(get(hObject, 'String')) returns contents of Linear_velocity as a double 301 | 302 | % --- Executes during object creation, after setting all properties. 303 | function Linear_velocity_CreateFcn(hObject, eventdata, handles) 304 | % hObject handle to Linear_velocity (see GCBO) 305 | % eventdata reserved - to be defined in a future version of MATLAB 306 | % handles empty - handles not created until after all CreateFcns called 307 | 308 | % Hint: edit controls usually have a white background on Windows. 309 | % See ISPC and COMPUTER. 310 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 311 | set(hObject, 'BackgroundColor', 'white'); 312 | end 313 | 314 | function Frequency_Callback(hObject, eventdata, handles) 315 | % hObject handle to Frequency (see GCBO) 316 | % eventdata reserved - to be defined in a future version of MATLAB 317 | % handles structure with handles and user data (see GUIDATA) 318 | 319 | % Hints: get(hObject, 'String') returns contents of Frequency as text 320 | % str2double(get(hObject, 'String')) returns contents of Frequency as a double 321 | 322 | % --- Executes during object creation, after setting all properties. 323 | function Frequency_CreateFcn(hObject, eventdata, handles) 324 | % hObject handle to Frequency (see GCBO) 325 | % eventdata reserved - to be defined in a future version of MATLAB 326 | % handles empty - handles not created until after all CreateFcns called 327 | 328 | % Hint: edit controls usually have a white background on Windows. 329 | % See ISPC and COMPUTER. 330 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 331 | set(hObject, 'BackgroundColor', 'white'); 332 | end 333 | 334 | function Landmark_X_Callback(hObject, eventdata, handles) 335 | % hObject handle to Landmark_X (see GCBO) 336 | % eventdata reserved - to be defined in a future version of MATLAB 337 | % handles structure with handles and user data (see GUIDATA) 338 | 339 | % Hints: get(hObject, 'String') returns contents of Landmark_X as text 340 | % str2double(get(hObject, 'String')) returns contents of Landmark_X as a double 341 | 342 | % --- Executes during object creation, after setting all properties. 343 | function Landmark_X_CreateFcn(hObject, eventdata, handles) 344 | % hObject handle to Landmark_X (see GCBO) 345 | % eventdata reserved - to be defined in a future version of MATLAB 346 | % handles empty - handles not created until after all CreateFcns called 347 | 348 | % Hint: edit controls usually have a white background on Windows. 349 | % See ISPC and COMPUTER. 350 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 351 | set(hObject, 'BackgroundColor', 'white'); 352 | end 353 | 354 | function Landmark_Y_Callback(hObject, eventdata, handles) 355 | % hObject handle to Landmark_Y (see GCBO) 356 | % eventdata reserved - to be defined in a future version of MATLAB 357 | % handles structure with handles and user data (see GUIDATA) 358 | 359 | % Hints: get(hObject, 'String') returns contents of Landmark_Y as text 360 | % str2double(get(hObject, 'String')) returns contents of Landmark_Y as a double 361 | 362 | % --- Executes during object creation, after setting all properties. 363 | function Landmark_Y_CreateFcn(hObject, eventdata, handles) 364 | % hObject handle to Landmark_Y (see GCBO) 365 | % eventdata reserved - to be defined in a future version of MATLAB 366 | % handles empty - handles not created until after all CreateFcns called 367 | 368 | % Hint: edit controls usually have a white background on Windows. 369 | % See ISPC and COMPUTER. 370 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 371 | set(hObject, 'BackgroundColor', 'white'); 372 | end 373 | 374 | % --- Executes on button press in Play. 375 | function Play_Callback(hObject, eventdata, handles) 376 | % hObject handle to Play (see GCBO) 377 | % eventdata reserved - to be defined in a future version of MATLAB 378 | % handles structure with handles and user data (see GUIDATA) 379 | cla(handles.Plot, 'reset'); 380 | set(handles.Plot, 'Color', [0.3 0.3 0.3]); 381 | zoom off; 382 | pan off; 383 | set(handles.Default, 'Enable', 'OFF'); 384 | disable_fields(hObject, handles); 385 | set_Legend(hObject, handles); 386 | 387 | [mu0, X0, theta, r, q, Sigma0, vl, freq, L, iter] = create_var(hObject, handles); 388 | Esercitazione2(mu0, X0, theta, r, q, Sigma0, vl, freq, L, iter, hObject, handles); 389 | 390 | set(handles.Default, 'Enable', 'ON'); 391 | flag_default = get(handles.Default, 'Value'); 392 | if (~flag_default) 393 | enable_fields(hObject, handles); 394 | end 395 | 396 | function set_Legend(hObject, handles) 397 | im = imread('legend2.png'); 398 | set(handles.Legend, 'HandleVisibility', 'ON'); 399 | axes(handles.Legend); 400 | image(im); 401 | set(handles.Legend, 'XTick', []); 402 | set(handles.Legend, 'YTick', []); 403 | 404 | function steps_Callback(hObject, eventdata, handles) 405 | % hObject handle to steps (see GCBO) 406 | % eventdata reserved - to be defined in a future version of MATLAB 407 | % handles structure with handles and user data (see GUIDATA) 408 | 409 | % Hints: get(hObject, 'String') returns contents of steps as text 410 | % str2double(get(hObject, 'String')) returns contents of steps as a double 411 | 412 | % --- Executes during object creation, after setting all properties. 413 | function steps_CreateFcn(hObject, eventdata, handles) 414 | % hObject handle to steps (see GCBO) 415 | % eventdata reserved - to be defined in a future version of MATLAB 416 | % handles empty - handles not created until after all CreateFcns called 417 | 418 | % Hint: edit controls usually have a white background on Windows. 419 | % See ISPC and COMPUTER. 420 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 421 | set(hObject, 'BackgroundColor', 'white'); 422 | end 423 | 424 | % --- Executes on button press in Stop. 425 | function Stop_Callback(hObject, eventdata, handles) 426 | % hObject handle to Stop (see GCBO) 427 | % eventdata reserved - to be defined in a future version of MATLAB 428 | % handles structure with handles and user data (see GUIDATA) 429 | global flag_stop; 430 | flag_stop = 1; 431 | set(handles.Default, 'Enable', 'ON'); 432 | flag_default = get(handles.Default, 'Value'); 433 | if (~flag_default) 434 | enable_fields(hObject, handles); 435 | end 436 | 437 | % --- Executes on button press in Reset. 438 | function Reset_Callback(hObject, eventdata, handles) 439 | % hObject handle to Reset (see GCBO) 440 | % eventdata reserved - to be defined in a future version of MATLAB 441 | % handles structure with handles and user data (see GUIDATA) 442 | Stop_Callback(hObject, eventdata, handles); 443 | zoom off; 444 | pan off; 445 | h = initialize_gui(hObject, handles); 446 | set(handles.Legend, 'XTick', []); 447 | set(handles.Legend, 'YTick', []); 448 | 449 | % --- Executes on button press in Zoom_in. 450 | function Zoom_in_Callback(hObject, eventdata, handles) 451 | % hObject handle to Zoom_in (see GCBO) 452 | % eventdata reserved - to be defined in a future version of MATLAB 453 | % handles structure with handles and user data (see GUIDATA) 454 | 455 | % --- Executes on button press in Zoom_out. 456 | function Zoom_out_Callback(hObject, eventdata, handles) 457 | % hObject handle to Zoom_out (see GCBO) 458 | % eventdata reserved - to be defined in a future version of MATLAB 459 | % handles structure with handles and user data (see GUIDATA) 460 | 461 | % --- Executes on button press in Zoom. 462 | function Zoom_Callback(hObject, eventdata, handles) 463 | % hObject handle to Zoom (see GCBO) 464 | % eventdata reserved - to be defined in a future version of MATLAB 465 | % handles structure with handles and user data (see GUIDATA) 466 | zoom on; 467 | 468 | % --- Executes on button press in Pan. 469 | function Pan_Callback(hObject, eventdata, handles) 470 | % hObject handle to Pan (see GCBO) 471 | % eventdata reserved - to be defined in a future version of MATLAB 472 | % handles structure with handles and user data (see GUIDATA) 473 | pan on; 474 | 475 | function handles = initialize_gui(hObject, handles) 476 | % If the metricdata field is present and the reset flag is false, it means 477 | % we are we are just re-initializing a GUI by calling it from the cmd line 478 | % while it is up. So, bail out as we dont want to reset the data. 479 | cla(handles.Plot, 'reset'); 480 | set(handles.Plot, 'Color', [0.3 0.3 0.3]); 481 | cla(handles.Legend, 'reset'); 482 | set(handles.Legend, 'Color', [0.3 0.3 0.3]); 483 | set(handles.Legend, 'Visible', 'OFF'); 484 | set(handles.Legend, 'XTick', []); 485 | set(handles.Legend, 'YTick', []); 486 | set(handles.Default, 'Enable', 'ON'); 487 | set(handles.Default, 'Value', 1); 488 | set_Default(hObject, handles); 489 | disable_fields(hObject, handles); 490 | 491 | function set_Default(hObject, handles) 492 | set(handles.Mu0_X, 'String', -0.5); 493 | set(handles.Mu0_Y, 'String', -1.5); 494 | set(handles.X0_X, 'String', -1); 495 | set(handles.X0_Y, 'String', -2); 496 | set(handles.Direction, 'String', -0.52); 497 | set(handles.Control_noise, 'String', 0); 498 | set(handles.Sensor_noise, 'String', 0.33); 499 | set(handles.Sigma_stdX, 'String', 100); 500 | set(handles.Sigma_stdY, 'String', 100); 501 | set(handles.Linear_velocity, 'String', 4.5); 502 | set(handles.Frequency, 'String', 50); 503 | set(handles.Landmark_X, 'String', 8); 504 | set(handles.Landmark_Y, 'String', 0); 505 | set(handles.steps, 'String', 200); 506 | 507 | function disable_fields(hObject, handles) 508 | set(handles.Mu0_X, 'Enable', 'OFF'); 509 | set(handles.Mu0_Y, 'Enable', 'OFF'); 510 | set(handles.X0_X, 'Enable', 'OFF'); 511 | set(handles.X0_Y, 'Enable', 'OFF'); 512 | set(handles.Direction, 'Enable', 'OFF'); 513 | set(handles.Control_noise, 'Enable', 'OFF'); 514 | set(handles.Sensor_noise, 'Enable', 'OFF'); 515 | set(handles.Sigma_stdX, 'Enable', 'OFF'); 516 | set(handles.Sigma_stdY, 'Enable', 'OFF'); 517 | set(handles.Linear_velocity, 'Enable', 'OFF'); 518 | set(handles.Frequency, 'Enable', 'OFF'); 519 | set(handles.Landmark_X, 'Enable', 'OFF'); 520 | set(handles.Landmark_Y, 'Enable', 'OFF'); 521 | set(handles.steps, 'Enable', 'OFF'); 522 | 523 | function enable_fields(hObject, handles) 524 | set(handles.Mu0_X, 'Enable', 'ON'); 525 | set(handles.Mu0_Y, 'Enable', 'ON'); 526 | set(handles.X0_X, 'Enable', 'ON'); 527 | set(handles.X0_Y, 'Enable', 'ON'); 528 | set(handles.Direction, 'Enable', 'ON'); 529 | set(handles.Control_noise, 'Enable', 'ON'); 530 | set(handles.Sensor_noise, 'Enable', 'ON'); 531 | set(handles.Sigma_stdX, 'Enable', 'ON'); 532 | set(handles.Sigma_stdY, 'Enable', 'ON'); 533 | set(handles.Linear_velocity, 'Enable', 'ON'); 534 | set(handles.Frequency, 'Enable', 'ON'); 535 | set(handles.Landmark_X, 'Enable', 'ON'); 536 | set(handles.Landmark_Y, 'Enable', 'ON'); 537 | set(handles.steps, 'Enable', 'ON'); 538 | 539 | function [mu0, X0, theta, r, q, Sigma0, vl, freq, L, iter] = create_var(hObject, handles) 540 | Mu0_X = eval(get(handles.Mu0_X, 'String')); 541 | Mu0_Y = eval(get(handles.Mu0_Y, 'String')); 542 | X0_X = eval(get(handles.X0_X, 'String')); 543 | X0_Y = eval(get(handles.X0_Y, 'String')); 544 | Direction = eval(get(handles.Direction, 'String')); 545 | Control_noise = eval(get(handles.Control_noise, 'String')); 546 | Sensor_noise = eval(get(handles.Sensor_noise, 'String')); 547 | Sigma_stdX = eval(get(handles.Sigma_stdX, 'String')); 548 | Sigma_stdY = eval(get(handles.Sigma_stdY, 'String')); 549 | Linear_velocity = eval(get(handles.Linear_velocity, 'String')); 550 | Frequency = eval(get(handles.Frequency, 'String')); 551 | Landmark_X = eval(get(handles.Landmark_X, 'String')); 552 | Landmark_Y = eval(get(handles.Landmark_Y, 'String')); 553 | steps = eval(get(handles.steps, 'String')); 554 | 555 | mu0 = [Mu0_X; Mu0_Y]; 556 | X0 = [X0_X; X0_Y]; 557 | theta = Direction; 558 | r = Control_noise; 559 | if (Sensor_noise <= 2e-5) 560 | Sensor_noise = 2e-5; 561 | end 562 | q = Sensor_noise^2; 563 | Sigma0 = [Sigma_stdX, 0; 0, Sigma_stdY]; 564 | vl = Linear_velocity; 565 | freq = Frequency; 566 | L = [Landmark_X; Landmark_Y]; 567 | iter = steps; 568 | 569 | function Esercitazione2(mu0, X0, theta, r, q, Sigma0, vl, freq, L, iter, hObject, handles) 570 | %La Funzione implementa il Filtro di Kalman esteso con landmark 571 | 572 | global flag_stop; 573 | flag_stop = 0; 574 | axes(handles.Plot); 575 | 576 | cov_gain = 10; 577 | dt = 1/freq; %Periodo di campionamento 578 | U = [vl; vl]; %Vettore di controllo sul robot 579 | I = eye(2); %Matrice identita' utilizzata nel filtro di Kalman esteso 580 | 581 | A = [1 0; 0 1]; 582 | B = [cos(theta)*dt 0; 0 sin(theta)*dt]; 583 | R = r*eye(2); %matrice di covarianza del processo 584 | 585 | %Inizializzazione algoritmo 586 | Xk = X0; 587 | muk = mu0; 588 | Sigmak = Sigma0; 589 | matrix_err = zeros(iter, 1); 590 | matrix_mu_pred = zeros(iter, 2); 591 | matrix_xk = zeros(iter, 2); 592 | matrix_muk = zeros(iter, 2); 593 | matrix_sigmak = zeros(2, 2, iter); 594 | 595 | for i = 1 : iter 596 | 597 | %Il rumore generato e' a media 0 e varianza pari a q 598 | Q = (0 + sqrt(q) .* randn(1)); 599 | 600 | Xk = (A*Xk)+(B*U); %Posizione reale del robot 601 | 602 | Zk = 2*sqrt((Xk-L)'*(Xk-L)) + Q; %Misura della distanza con aggiunta di rumore 603 | Z_pred = 2*sqrt((muk-L)'*(muk-L)); %Misura 604 | 605 | H = (2*(muk-L)/(Zk/2))'; %Jacobiano 606 | 607 | %Fase di Predizione 608 | mu_pred = (A*muk + B*U); 609 | Sigma_pred = A*(Sigmak*A') + R; 610 | 611 | %Fase di Correzione 612 | S = symmetric_m(H*(Sigma_pred*H') + q); 613 | %K_gain = Sigma_pred*(H'*inv(S)); %Guadagno di Kalman 614 | K_gain = Sigma_pred*(H'/S); %Guadagno di Kalman 615 | muk = mu_pred+K_gain*(Zk-Z_pred); %Posizione stimata del robot 616 | Sigmak = (I-K_gain*H)*Sigma_pred; %Matrice di covarianza stimata del robot 617 | Sigmak = symmetric_m(Sigmak); 618 | 619 | %Calcolo distanza tra posizione reale e stimata 620 | errore = sqrt((Xk-muk)'*(Xk-muk)); 621 | 622 | %Memorizzazione dati 623 | matrix_xk(i, :) = Xk(:, :)'; 624 | matrix_mu_pred(i, :) = mu_pred(:, :)'; 625 | matrix_muk(i, :) = muk(:, :)'; 626 | matrix_err(i, :) = errore; 627 | matrix_sigmak(:, :, i) = Sigmak; 628 | 629 | end 630 | 631 | %Calcolo dei limiti della finestra di visualizzazione 632 | x_min = min(matrix_xk(:, 1)); 633 | x_max = max(matrix_xk(:, 1)); 634 | y_min = min(matrix_xk(:, 2)); 635 | y_max = max(matrix_xk(:, 2)); 636 | x_min_ass = min(min(x_min, min(matrix_muk(:, 1))), L(1)); 637 | x_max_ass = max(max(x_max, max(matrix_muk(:, 1))), L(1)); 638 | y_min_ass = min(min(y_min, min(matrix_muk(:, 2))), L(2)); 639 | y_max_ass = max(max(y_max, max(matrix_muk(:, 2))), L(2)); 640 | 641 | %Plot dei parametri 642 | set(gca, 'Color', [0.3 0.3 0.3]); 643 | lim_axis = [x_min_ass-5 x_max_ass+5 y_min_ass-5 y_max_ass+5]; 644 | axis(lim_axis); 645 | x_step = (lim_axis(2)-lim_axis(1))/5; 646 | y_step = (lim_axis(4)-lim_axis(3))/5; 647 | set(handles.Plot, 'XTick', round(100*(lim_axis(1):x_step:lim_axis(2)))/100); 648 | set(handles.Plot, 'YTick', round(100*(lim_axis(3):y_step:lim_axis(4)))/100); 649 | hold on; 650 | scatter(L(1, 1), L(2, 1), 'marker', 'o', 'markerEdgecolor', [0 0 1], 'markerfacecolor', [0 0 1]); 651 | plot(X0(1, 1), X0(2, 1), 'xc'); 652 | plot(mu0(1, 1), mu0(2, 1), 'or'); 653 | plot(matrix_muk(1, 1), matrix_muk(1, 2), 'xr'); 654 | plot(matrix_xk(:, 1), matrix_xk(:, 2), 'c'); 655 | h2 = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'ro', 'markersize', 5, 'markerfacecolor', 'r'); 656 | vcov = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'r'); 657 | 658 | %Rappresentazione puntuale dell'andamento stimato del robot 659 | fastdraw = true; 660 | if (fastdraw) 661 | drawstep = 2; %Parametro proporzionale alla velocita' di rappresentazione (>1) 662 | for i = 1+drawstep : drawstep-1 : size(matrix_muk, 1) 663 | h1 = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'r.-'); 664 | set(h2, 'XData', matrix_muk(i, 1), 'YData', matrix_muk(i, 2)); 665 | set(h1, 'XData', matrix_muk(i-drawstep:i, 1), 'YData', matrix_muk(i-drawstep:i, 2)); 666 | if (sum(sum(matrix_sigmak(:, :, i) == zeros(2))) ~= 4) 667 | pvcov = PlotEllipse(matrix_muk(i, :)', matrix_sigmak(:, :, i), cov_gain); 668 | set(vcov, 'xdata', pvcov(1, :), 'ydata', pvcov(2, :)); 669 | end 670 | drawnow; 671 | if (flag_stop) 672 | return 673 | end 674 | end 675 | else 676 | for i = 1 : size(matrix_muk, 1) 677 | h1 = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'r.-'); 678 | set(h2, 'XData', matrix_muk(i, 1), 'YData', matrix_muk(i, 2)); 679 | set(h1, 'XData', matrix_muk(i, 1), 'YData', matrix_muk(i, 2)); 680 | if (sum(sum(matrix_sigmak(:, :, i) == zeros(2))) ~= 4) 681 | pvcov = PlotEllipse(matrix_muk(i, :)', matrix_sigmak(:, :, i), cov_gain); 682 | set(vcov, 'xdata', pvcov(1, :), 'ydata', pvcov(2, :)); 683 | end 684 | drawnow; 685 | if (flag_stop) 686 | return 687 | end 688 | end 689 | end 690 | 691 | %h = legend('Landmark', 'X0', 'mu0', 'mkinit', 'Xk', 'Robot', 'Mk', 'Location', 'Best'); 692 | 693 | function S = symmetric_m(S) 694 | % Funzione per mantenere la forma simmetrica di una matrice ed ovviare ad 695 | % eventuali errori di arrotondamento scaturiti dai calcoli 696 | S = (S+S')*0.5; 697 | 698 | function el = PlotEllipse(x, P, nSigma) 699 | % Funzione per il tracciamento di una singola ellisse di covarianza 700 | N = 8; 701 | step = 2*pi/N; 702 | if (all(diag(P))) 703 | [V, D] = eig(P); 704 | y = nSigma * [cos(0:step:2*pi); sin(0:step:2*pi)]; 705 | el = V*sqrtm(D)*y; 706 | el = [el el(:, 1)] + repmat(x, 1, size(el, 2)+1); 707 | el(:, end) = NaN; 708 | end 709 | -------------------------------------------------------------------------------- /01-KalmanFilter/KalmanF01.m: -------------------------------------------------------------------------------- 1 | function varargout = KalmanF01(varargin) 2 | % KALMANF01 MATLAB code for KalmanF01.fig 3 | % KALMANF01, by itself, creates a new KALMANF01 or raises the existing 4 | % singleton*. 5 | % 6 | % H = KALMANF01 returns the handle to a new KALMANF01 or the handle to 7 | % the existing singleton*. 8 | % 9 | % KALMANF01('CALLBACK', hObject, eventData, handles, ...) calls the local 10 | % function named CALLBACK in KALMANF01.M with the given input arguments. 11 | % 12 | % KALMANF01('Property', 'Value', ...) creates a new KALMANF01 or raises the 13 | % existing singleton*. Starting from the left, property value pairs are 14 | % applied to the GUI before KalmanF01_OpeningFcn gets called. An 15 | % unrecognized property name or invalid value makes property application 16 | % stop. All inputs are passed to KalmanF01_OpeningFcn via varargin. 17 | % 18 | % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one 19 | % instance to run (singleton)". 20 | % 21 | % See also: GUIDE, GUIDATA, GUIHANDLES 22 | % 23 | % -- 24 | % Salvatore La Bua (slabua@gmail.com) 25 | % GLB 26 | % MTM 27 | % DICGIM - University of Palermo 28 | 29 | % Edit the above text to modify the response to help KalmanF01 30 | 31 | % Last Modified by GUIDE v2.5 16-Oct-2013 01:45:32 32 | 33 | % Begin initialization code - DO NOT EDIT 34 | gui_Singleton = 1; 35 | gui_State = struct('gui_Name', mfilename, ... 36 | 'gui_Singleton', gui_Singleton, ... 37 | 'gui_OpeningFcn', @KalmanF01_OpeningFcn, ... 38 | 'gui_OutputFcn', @KalmanF01_OutputFcn, ... 39 | 'gui_LayoutFcn', [], ... 40 | 'gui_Callback', []); 41 | if nargin && ischar(varargin{1}) 42 | gui_State.gui_Callback = str2func(varargin{1}); 43 | end 44 | 45 | if nargout 46 | [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); 47 | else 48 | gui_mainfcn(gui_State, varargin{:}); 49 | end 50 | % End initialization code - DO NOT EDIT 51 | 52 | % --- Executes just before KalmanF01 is made visible. 53 | function KalmanF01_OpeningFcn(hObject, eventdata, handles, varargin) 54 | % This function has no output args, see OutputFcn. 55 | % hObject handle to figure 56 | % eventdata reserved - to be defined in a future version of MATLAB 57 | % handles structure with handles and user data (see GUIDATA) 58 | % varargin command line arguments to KalmanF01 (see VARARGIN) 59 | handles = initialize_gui(hObject, handles); 60 | % Choose default command line output for KalmanF01 61 | handles.output = hObject; 62 | 63 | % Update handles structure 64 | guidata(hObject, handles); 65 | 66 | % UIWAIT makes KalmanF01 wait for user response (see UIRESUME) 67 | % uiwait(handles.figure1); 68 | 69 | % --- Outputs from this function are returned to the command line. 70 | function varargout = KalmanF01_OutputFcn(hObject, eventdata, handles) 71 | % varargout cell array for returning output args (see VARARGOUT); 72 | % hObject handle to figure 73 | % eventdata reserved - to be defined in a future version of MATLAB 74 | % handles structure with handles and user data (see GUIDATA) 75 | 76 | % Get default command line output from handles structure 77 | varargout{1} = handles.output; 78 | 79 | % --- Executes on button press in Default. 80 | function Default_Callback(hObject, eventdata, handles) 81 | % hObject handle to Default (see GCBO) 82 | % eventdata reserved - to be defined in a future version of MATLAB 83 | % handles structure with handles and user data (see GUIDATA) 84 | flag_default = get(handles.Default, 'Value'); 85 | if (flag_default) 86 | set_Default(hObject, handles); 87 | disable_fields(hObject, handles); 88 | else 89 | set_Default(hObject, handles); 90 | enable_fields(hObject, handles); 91 | end 92 | % Hint: get(hObject, 'Value') returns toggle state of Default 93 | 94 | function Mu0_X_Callback(hObject, eventdata, handles) 95 | % hObject handle to Mu0_X (see GCBO) 96 | % eventdata reserved - to be defined in a future version of MATLAB 97 | % handles structure with handles and user data (see GUIDATA) 98 | 99 | % Hints: get(hObject, 'String') returns contents of Mu0_X as text 100 | % str2double(get(hObject, 'String')) returns contents of Mu0_X as a double 101 | 102 | % --- Executes during object creation, after setting all properties. 103 | function Mu0_X_CreateFcn(hObject, eventdata, handles) 104 | % hObject handle to Mu0_X (see GCBO) 105 | % eventdata reserved - to be defined in a future version of MATLAB 106 | % handles empty - handles not created until after all CreateFcns called 107 | 108 | % Hint: edit controls usually have a white background on Windows. 109 | % See ISPC and COMPUTER. 110 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 111 | set(hObject, 'BackgroundColor', 'white'); 112 | end 113 | 114 | function Mu0_Y_Callback(hObject, eventdata, handles) 115 | % hObject handle to Mu0_Y (see GCBO) 116 | % eventdata reserved - to be defined in a future version of MATLAB 117 | % handles structure with handles and user data (see GUIDATA) 118 | 119 | % Hints: get(hObject, 'String') returns contents of Mu0_Y as text 120 | % str2double(get(hObject, 'String')) returns contents of Mu0_Y as a double 121 | 122 | % --- Executes during object creation, after setting all properties. 123 | function Mu0_Y_CreateFcn(hObject, eventdata, handles) 124 | % hObject handle to Mu0_Y (see GCBO) 125 | % eventdata reserved - to be defined in a future version of MATLAB 126 | % handles empty - handles not created until after all CreateFcns called 127 | 128 | % Hint: edit controls usually have a white background on Windows. 129 | % See ISPC and COMPUTER. 130 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 131 | set(hObject, 'BackgroundColor', 'white'); 132 | end 133 | 134 | function X0_X_Callback(hObject, eventdata, handles) 135 | % hObject handle to X0_X (see GCBO) 136 | % eventdata reserved - to be defined in a future version of MATLAB 137 | % handles structure with handles and user data (see GUIDATA) 138 | 139 | % Hints: get(hObject, 'String') returns contents of X0_X as text 140 | % str2double(get(hObject, 'String')) returns contents of X0_X as a double 141 | 142 | % --- Executes during object creation, after setting all properties. 143 | function X0_X_CreateFcn(hObject, eventdata, handles) 144 | % hObject handle to X0_X (see GCBO) 145 | % eventdata reserved - to be defined in a future version of MATLAB 146 | % handles empty - handles not created until after all CreateFcns called 147 | 148 | % Hint: edit controls usually have a white background on Windows. 149 | % See ISPC and COMPUTER. 150 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 151 | set(hObject, 'BackgroundColor', 'white'); 152 | end 153 | 154 | function X0_Y_Callback(hObject, eventdata, handles) 155 | % hObject handle to X0_Y (see GCBO) 156 | % eventdata reserved - to be defined in a future version of MATLAB 157 | % handles structure with handles and user data (see GUIDATA) 158 | 159 | % Hints: get(hObject, 'String') returns contents of X0_Y as text 160 | % str2double(get(hObject, 'String')) returns contents of X0_Y as a double 161 | 162 | % --- Executes during object creation, after setting all properties. 163 | function X0_Y_CreateFcn(hObject, eventdata, handles) 164 | % hObject handle to X0_Y (see GCBO) 165 | % eventdata reserved - to be defined in a future version of MATLAB 166 | % handles empty - handles not created until after all CreateFcns called 167 | 168 | % Hint: edit controls usually have a white background on Windows. 169 | % See ISPC and COMPUTER. 170 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 171 | set(hObject, 'BackgroundColor', 'white'); 172 | end 173 | 174 | function Direction_Callback(hObject, eventdata, handles) 175 | % hObject handle to Direction (see GCBO) 176 | % eventdata reserved - to be defined in a future version of MATLAB 177 | % handles structure with handles and user data (see GUIDATA) 178 | 179 | % Hints: get(hObject, 'String') returns contents of Direction as text 180 | % str2double(get(hObject, 'String')) returns contents of Direction as a double 181 | 182 | % --- Executes during object creation, after setting all properties. 183 | function Direction_CreateFcn(hObject, eventdata, handles) 184 | % hObject handle to Direction (see GCBO) 185 | % eventdata reserved - to be defined in a future version of MATLAB 186 | % handles empty - handles not created until after all CreateFcns called 187 | 188 | % Hint: edit controls usually have a white background on Windows. 189 | % See ISPC and COMPUTER. 190 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 191 | set(hObject, 'BackgroundColor', 'white'); 192 | end 193 | 194 | function Control_noise_Callback(hObject, eventdata, handles) 195 | % hObject handle to Control_noise (see GCBO) 196 | % eventdata reserved - to be defined in a future version of MATLAB 197 | % handles structure with handles and user data (see GUIDATA) 198 | 199 | % Hints: get(hObject, 'String') returns contents of Control_noise as text 200 | % str2double(get(hObject, 'String')) returns contents of Control_noise as a double 201 | 202 | % --- Executes during object creation, after setting all properties. 203 | function Control_noise_CreateFcn(hObject, eventdata, handles) 204 | % hObject handle to Control_noise (see GCBO) 205 | % eventdata reserved - to be defined in a future version of MATLAB 206 | % handles empty - handles not created until after all CreateFcns called 207 | 208 | % Hint: edit controls usually have a white background on Windows. 209 | % See ISPC and COMPUTER. 210 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 211 | set(hObject, 'BackgroundColor', 'white'); 212 | end 213 | 214 | function Sensor_noise_Callback(hObject, eventdata, handles) 215 | % hObject handle to Sensor_noise (see GCBO) 216 | % eventdata reserved - to be defined in a future version of MATLAB 217 | % handles structure with handles and user data (see GUIDATA) 218 | 219 | % Hints: get(hObject, 'String') returns contents of Sensor_noise as text 220 | % str2double(get(hObject, 'String')) returns contents of Sensor_noise as a double 221 | 222 | % --- Executes during object creation, after setting all properties. 223 | function Sensor_noise_CreateFcn(hObject, eventdata, handles) 224 | % hObject handle to Sensor_noise (see GCBO) 225 | % eventdata reserved - to be defined in a future version of MATLAB 226 | % handles empty - handles not created until after all CreateFcns called 227 | 228 | % Hint: edit controls usually have a white background on Windows. 229 | % See ISPC and COMPUTER. 230 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 231 | set(hObject, 'BackgroundColor', 'white'); 232 | end 233 | 234 | function Sigma_stdX_Callback(hObject, eventdata, handles) 235 | % hObject handle to Sigma_stdX (see GCBO) 236 | % eventdata reserved - to be defined in a future version of MATLAB 237 | % handles structure with handles and user data (see GUIDATA) 238 | 239 | % Hints: get(hObject, 'String') returns contents of Sigma_stdX as text 240 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdX as a double 241 | 242 | % --- Executes during object creation, after setting all properties. 243 | function Sigma_stdX_CreateFcn(hObject, eventdata, handles) 244 | % hObject handle to Sigma_stdX (see GCBO) 245 | % eventdata reserved - to be defined in a future version of MATLAB 246 | % handles empty - handles not created until after all CreateFcns called 247 | 248 | % Hint: edit controls usually have a white background on Windows. 249 | % See ISPC and COMPUTER. 250 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 251 | set(hObject, 'BackgroundColor', 'white'); 252 | end 253 | 254 | function Sigma_stdY_Callback(hObject, eventdata, handles) 255 | % hObject handle to Sigma_stdX (see GCBO) 256 | % eventdata reserved - to be defined in a future version of MATLAB 257 | % handles structure with handles and user data (see GUIDATA) 258 | 259 | % Hints: get(hObject, 'String') returns contents of Sigma_stdX as text 260 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdX as a double 261 | 262 | % --- Executes during object creation, after setting all properties. 263 | function Sigma_stdY_CreateFcn(hObject, eventdata, handles) 264 | % hObject handle to Sigma_stdX (see GCBO) 265 | % eventdata reserved - to be defined in a future version of MATLAB 266 | % handles empty - handles not created until after all CreateFcns called 267 | 268 | % Hint: edit controls usually have a white background on Windows. 269 | % See ISPC and COMPUTER. 270 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 271 | set(hObject, 'BackgroundColor', 'white'); 272 | end 273 | 274 | function Sigma_std_Callback(hObject, eventdata, handles) 275 | % hObject handle to Sigma_stdY (see GCBO) 276 | % eventdata reserved - to be defined in a future version of MATLAB 277 | % handles structure with handles and user data (see GUIDATA) 278 | 279 | % Hints: get(hObject, 'String') returns contents of Sigma_stdY as text 280 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdY as a double 281 | 282 | % --- Executes during object creation, after setting all properties. 283 | function Sigma_std_CreateFcn(hObject, eventdata, handles) 284 | % hObject handle to Sigma_stdY (see GCBO) 285 | % eventdata reserved - to be defined in a future version of MATLAB 286 | % handles empty - handles not created until after all CreateFcns called 287 | 288 | % Hint: edit controls usually have a white background on Windows. 289 | % See ISPC and COMPUTER. 290 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 291 | set(hObject, 'BackgroundColor', 'white'); 292 | end 293 | 294 | function Linear_velocity_Callback(hObject, eventdata, handles) 295 | % hObject handle to Linear_velocity (see GCBO) 296 | % eventdata reserved - to be defined in a future version of MATLAB 297 | % handles structure with handles and user data (see GUIDATA) 298 | 299 | % Hints: get(hObject, 'String') returns contents of Linear_velocity as text 300 | % str2double(get(hObject, 'String')) returns contents of Linear_velocity as a double 301 | 302 | % --- Executes during object creation, after setting all properties. 303 | function Linear_velocity_CreateFcn(hObject, eventdata, handles) 304 | % hObject handle to Linear_velocity (see GCBO) 305 | % eventdata reserved - to be defined in a future version of MATLAB 306 | % handles empty - handles not created until after all CreateFcns called 307 | 308 | % Hint: edit controls usually have a white background on Windows. 309 | % See ISPC and COMPUTER. 310 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 311 | set(hObject, 'BackgroundColor', 'white'); 312 | end 313 | 314 | function Frequency_Callback(hObject, eventdata, handles) 315 | % hObject handle to Frequency (see GCBO) 316 | % eventdata reserved - to be defined in a future version of MATLAB 317 | % handles structure with handles and user data (see GUIDATA) 318 | 319 | % Hints: get(hObject, 'String') returns contents of Frequency as text 320 | % str2double(get(hObject, 'String')) returns contents of Frequency as a double 321 | 322 | % --- Executes during object creation, after setting all properties. 323 | function Frequency_CreateFcn(hObject, eventdata, handles) 324 | % hObject handle to Frequency (see GCBO) 325 | % eventdata reserved - to be defined in a future version of MATLAB 326 | % handles empty - handles not created until after all CreateFcns called 327 | 328 | % Hint: edit controls usually have a white background on Windows. 329 | % See ISPC and COMPUTER. 330 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 331 | set(hObject, 'BackgroundColor', 'white'); 332 | end 333 | 334 | function Wall_parameters_m_Callback(hObject, eventdata, handles) 335 | % hObject handle to Wall_parameters_m (see GCBO) 336 | % eventdata reserved - to be defined in a future version of MATLAB 337 | % handles structure with handles and user data (see GUIDATA) 338 | 339 | % Hints: get(hObject, 'String') returns contents of Wall_parameters_m as text 340 | % str2double(get(hObject, 'String')) returns contents of Wall_parameters_m as a double 341 | 342 | % --- Executes during object creation, after setting all properties. 343 | function Wall_parameters_m_CreateFcn(hObject, eventdata, handles) 344 | % hObject handle to Wall_parameters_m (see GCBO) 345 | % eventdata reserved - to be defined in a future version of MATLAB 346 | % handles empty - handles not created until after all CreateFcns called 347 | 348 | % Hint: edit controls usually have a white background on Windows. 349 | % See ISPC and COMPUTER. 350 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 351 | set(hObject, 'BackgroundColor', 'white'); 352 | end 353 | 354 | function Wall_parameters_q_Callback(hObject, eventdata, handles) 355 | % hObject handle to Wall_parameters_q (see GCBO) 356 | % eventdata reserved - to be defined in a future version of MATLAB 357 | % handles structure with handles and user data (see GUIDATA) 358 | 359 | % Hints: get(hObject, 'String') returns contents of Wall_parameters_q as text 360 | % str2double(get(hObject, 'String')) returns contents of Wall_parameters_q as a double 361 | 362 | % --- Executes during object creation, after setting all properties. 363 | function Wall_parameters_q_CreateFcn(hObject, eventdata, handles) 364 | % hObject handle to Wall_parameters_q (see GCBO) 365 | % eventdata reserved - to be defined in a future version of MATLAB 366 | % handles empty - handles not created until after all CreateFcns called 367 | 368 | % Hint: edit controls usually have a white background on Windows. 369 | % See ISPC and COMPUTER. 370 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 371 | set(hObject, 'BackgroundColor', 'white'); 372 | end 373 | 374 | % --- Executes on button press in Play. 375 | function Play_Callback(hObject, eventdata, handles) 376 | % hObject handle to Play (see GCBO) 377 | % eventdata reserved - to be defined in a future version of MATLAB 378 | % handles structure with handles and user data (see GUIDATA) 379 | cla(handles.Plot, 'reset'); 380 | set(handles.Plot, 'Color', [0.3 0.3 0.3]); 381 | zoom off; 382 | pan off; 383 | set(handles.Default, 'Enable', 'OFF'); 384 | disable_fields(hObject, handles); 385 | set_Legend(hObject, handles); 386 | 387 | [mu0, X0, theta, r, q, Sigma0, vl, freq, m, b, iter] = create_var(hObject, handles); 388 | Esercitazione1(mu0, X0, theta, r, q, Sigma0, vl, freq, m, b, iter, hObject, handles); 389 | 390 | set(handles.Default, 'Enable', 'ON'); 391 | flag_default = get(handles.Default, 'Value'); 392 | if (~flag_default) 393 | enable_fields(hObject, handles); 394 | end 395 | 396 | function set_Legend(hObject, handles) 397 | im = imread('legend1.png'); 398 | set(handles.Legend, 'HandleVisibility', 'ON'); 399 | axes(handles.Legend); 400 | image(im); 401 | set(handles.Legend, 'XTick', []); 402 | set(handles.Legend, 'YTick', []); 403 | 404 | function steps_Callback(hObject, eventdata, handles) 405 | % hObject handle to steps (see GCBO) 406 | % eventdata reserved - to be defined in a future version of MATLAB 407 | % handles structure with handles and user data (see GUIDATA) 408 | 409 | % Hints: get(hObject, 'String') returns contents of steps as text 410 | % str2double(get(hObject, 'String')) returns contents of steps as a double 411 | 412 | % --- Executes during object creation, after setting all properties. 413 | function steps_CreateFcn(hObject, eventdata, handles) 414 | % hObject handle to steps (see GCBO) 415 | % eventdata reserved - to be defined in a future version of MATLAB 416 | % handles empty - handles not created until after all CreateFcns called 417 | 418 | % Hint: edit controls usually have a white background on Windows. 419 | % See ISPC and COMPUTER. 420 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 421 | set(hObject, 'BackgroundColor', 'white'); 422 | end 423 | 424 | % --- Executes on button press in Stop. 425 | function Stop_Callback(hObject, eventdata, handles) 426 | % hObject handle to Stop (see GCBO) 427 | % eventdata reserved - to be defined in a future version of MATLAB 428 | % handles structure with handles and user data (see GUIDATA) 429 | global flag_stop; 430 | flag_stop = 1; 431 | set(handles.Default, 'Enable', 'ON'); 432 | flag_default = get(handles.Default, 'Value'); 433 | if (~flag_default) 434 | enable_fields(hObject, handles); 435 | end 436 | 437 | % --- Executes on button press in Reset. 438 | function Reset_Callback(hObject, eventdata, handles) 439 | % hObject handle to Reset (see GCBO) 440 | % eventdata reserved - to be defined in a future version of MATLAB 441 | % handles structure with handles and user data (see GUIDATA) 442 | Stop_Callback(hObject, eventdata, handles); 443 | zoom off; 444 | pan off; 445 | h = initialize_gui(hObject, handles); 446 | set(handles.Legend, 'XTick', []); 447 | set(handles.Legend, 'YTick', []); 448 | 449 | % --- Executes on button press in Zoom_in. 450 | function Zoom_in_Callback(hObject, eventdata, handles) 451 | % hObject handle to Zoom_in (see GCBO) 452 | % eventdata reserved - to be defined in a future version of MATLAB 453 | % handles structure with handles and user data (see GUIDATA) 454 | 455 | % --- Executes on button press in Zoom_out. 456 | function Zoom_out_Callback(hObject, eventdata, handles) 457 | % hObject handle to Zoom_out (see GCBO) 458 | % eventdata reserved - to be defined in a future version of MATLAB 459 | % handles structure with handles and user data (see GUIDATA) 460 | 461 | % --- Executes on button press in Zoom. 462 | function Zoom_Callback(hObject, eventdata, handles) 463 | % hObject handle to Zoom (see GCBO) 464 | % eventdata reserved - to be defined in a future version of MATLAB 465 | % handles structure with handles and user data (see GUIDATA) 466 | zoom on; 467 | 468 | % --- Executes on button press in Pan. 469 | function Pan_Callback(hObject, eventdata, handles) 470 | % hObject handle to Pan (see GCBO) 471 | % eventdata reserved - to be defined in a future version of MATLAB 472 | % handles structure with handles and user data (see GUIDATA) 473 | pan on; 474 | 475 | function handles = initialize_gui(hObject, handles) 476 | % If the metricdata field is present and the reset flag is false, it means 477 | % we are we are just re-initializing a GUI by calling it from the cmd line 478 | % while it is up. So, bail out as we dont want to reset the data. 479 | cla(handles.Plot, 'reset'); 480 | set(handles.Plot, 'Color', [0.3 0.3 0.3]); 481 | cla(handles.Legend, 'reset'); 482 | set(handles.Legend, 'Color', [0.3 0.3 0.3]); 483 | set(handles.Legend, 'Visible', 'OFF'); 484 | set(handles.Legend, 'XTick', []); 485 | set(handles.Legend, 'YTick', []); 486 | set(handles.Default, 'Enable', 'ON'); 487 | set(handles.Default, 'Value', 1); 488 | set_Default(hObject, handles); 489 | disable_fields(hObject, handles); 490 | 491 | function set_Default(hObject, handles) 492 | set(handles.Mu0_X, 'String', 7); 493 | set(handles.Mu0_Y, 'String', -5); 494 | set(handles.X0_X, 'String', -1); 495 | set(handles.X0_Y, 'String', 0); 496 | set(handles.Direction, 'String', -0.52); 497 | set(handles.Control_noise, 'String', 0); 498 | set(handles.Sensor_noise, 'String', 0.33); 499 | set(handles.Sigma_stdX, 'String', 100); 500 | set(handles.Sigma_stdY, 'String', 100); 501 | set(handles.Linear_velocity, 'String', 4.5); 502 | set(handles.Frequency, 'String', 50); 503 | set(handles.Wall_parameters_m, 'String', 0); 504 | set(handles.Wall_parameters_q, 'String', 2); 505 | set(handles.steps, 'String', 200); 506 | 507 | function disable_fields(hObject, handles) 508 | set(handles.Mu0_X, 'Enable', 'OFF'); 509 | set(handles.Mu0_Y, 'Enable', 'OFF'); 510 | set(handles.X0_X, 'Enable', 'OFF'); 511 | set(handles.X0_Y, 'Enable', 'OFF'); 512 | set(handles.Direction, 'Enable', 'OFF'); 513 | set(handles.Control_noise, 'Enable', 'OFF'); 514 | set(handles.Sensor_noise, 'Enable', 'OFF'); 515 | set(handles.Sigma_stdX, 'Enable', 'OFF'); 516 | set(handles.Sigma_stdY, 'Enable', 'OFF'); 517 | set(handles.Linear_velocity, 'Enable', 'OFF'); 518 | set(handles.Frequency, 'Enable', 'OFF'); 519 | set(handles.Wall_parameters_m, 'Enable', 'OFF'); 520 | set(handles.Wall_parameters_q, 'Enable', 'OFF'); 521 | set(handles.steps, 'Enable', 'OFF'); 522 | 523 | function enable_fields(hObject, handles) 524 | set(handles.Mu0_X, 'Enable', 'ON'); 525 | set(handles.Mu0_Y, 'Enable', 'ON'); 526 | set(handles.X0_X, 'Enable', 'ON'); 527 | set(handles.X0_Y, 'Enable', 'ON'); 528 | set(handles.Direction, 'Enable', 'ON'); 529 | set(handles.Control_noise, 'Enable', 'ON'); 530 | set(handles.Sensor_noise, 'Enable', 'ON'); 531 | set(handles.Sigma_stdX, 'Enable', 'ON'); 532 | set(handles.Sigma_stdY, 'Enable', 'ON'); 533 | set(handles.Linear_velocity, 'Enable', 'ON'); 534 | set(handles.Frequency, 'Enable', 'ON'); 535 | set(handles.Wall_parameters_m, 'Enable', 'ON'); 536 | set(handles.Wall_parameters_q, 'Enable', 'ON'); 537 | set(handles.steps, 'Enable', 'ON'); 538 | 539 | function [mu0, X0, theta, r, q, Sigma0, vl, freq, m, b, iter] = create_var(hObject, handles) 540 | Mu0_X = eval(get(handles.Mu0_X, 'String')); 541 | Mu0_Y = eval(get(handles.Mu0_Y, 'String')); 542 | X0_X = eval(get(handles.X0_X, 'String')); 543 | X0_Y = eval(get(handles.X0_Y, 'String')); 544 | Direction = eval(get(handles.Direction, 'String')); 545 | Control_noise = eval(get(handles.Control_noise, 'String')); 546 | Sensor_noise = eval(get(handles.Sensor_noise, 'String')); 547 | Sigma_stdX = eval(get(handles.Sigma_stdX, 'String')); 548 | Sigma_stdY = eval(get(handles.Sigma_stdY, 'String')); 549 | Linear_velocity = eval(get(handles.Linear_velocity, 'String')); 550 | Frequency = eval(get(handles.Frequency, 'String')); 551 | Wall_parameters_m = eval(get(handles.Wall_parameters_m, 'String')); 552 | Wall_parameters_q = eval(get(handles.Wall_parameters_q, 'String')); 553 | steps = eval(get(handles.steps, 'String')); 554 | 555 | mu0 = [Mu0_X; Mu0_Y]; 556 | X0 = [X0_X; X0_Y]; 557 | theta = Direction; 558 | r = Control_noise; 559 | if (Sensor_noise <= 2e-5) 560 | Sensor_noise = 2e-5; 561 | end 562 | q = Sensor_noise^2; 563 | Sigma0 = [Sigma_stdX, 0; 0, Sigma_stdY]; 564 | vl = Linear_velocity; 565 | freq = Frequency; 566 | m = Wall_parameters_m; 567 | b = Wall_parameters_q; 568 | iter = steps; 569 | 570 | function Esercitazione1(mu0, X0, theta, r, q, Sigma0, vl, freq, m, b, iter, hObject, handles) 571 | %La Funzione implementa il Filtro di Kalman 572 | 573 | global flag_stop; 574 | flag_stop = 0; 575 | axes(handles.Plot); 576 | 577 | cov_gain = 1; 578 | dt = 1/freq; %Periodo di campionamento 579 | u = [vl; vl]; %Vettore di controllo sul robot 580 | I = eye(2); %Matrice identita' utilizzata nel filtro di Kalman 581 | 582 | A = [1 0; 0 1]; 583 | B = [cos(theta)*dt 0; 0 sin(theta)*dt]; 584 | C = [-2*m/(sqrt(m*m+1)), 2/(sqrt(m*m+1))]; 585 | 586 | R = r*eye(2); %matrice di covarianza del processo 587 | 588 | %Inizializzazione algoritmo 589 | Xk = X0; 590 | Sigmak = Sigma0; 591 | muk = mu0; 592 | matrix_err = zeros(iter, 1); 593 | matrix_mu_pred = zeros(iter, 2); 594 | matrix_xk = zeros(iter, 2); 595 | matrix_muk = zeros(iter, 2); 596 | matrix_sigmak = zeros(2, 2, iter); 597 | 598 | for i = 1 : iter 599 | 600 | %distanza tra la posizione reale del robot e il muro 601 | dist_PR = (-m*Xk(1, 1) + Xk(2, 1) - b) / sqrt( (-m)^2 +1 ); 602 | 603 | %Matrice di covarianza della misura 604 | %Il rumore generato e' a media 0 e varianza pari a q 605 | Q = (0 + sqrt(q) .* randn(1)); 606 | Xk = (A*Xk)+(B*u); %Posizione reale del robot 607 | Z_temp = 2*dist_PR + Q; %Misura della distanza con aggiunta di rumore 608 | Zk = Z_temp - 2*(-b/sqrt(1+m*m)); %Misura 609 | 610 | %Fase di Predizione 611 | mu_pred = (A*muk + B*u); 612 | Sigma_pred = A*(Sigmak*A') + R; 613 | 614 | %Fase di Correzione 615 | S = symmetric_m(C*(Sigma_pred*C') + q); 616 | %K_gain = Sigma_pred*(C'*inv(S)); %Guadagno di Kalman 617 | K_gain = Sigma_pred*(C'/S); %Guadagno di Kalman 618 | muk = mu_pred+K_gain*(Zk-C*mu_pred); %Posizione stimata del robot 619 | Sigmak = (I-K_gain*C)*Sigma_pred; %Matrice di covarianza stimata del robot 620 | Sigmak = symmetric_m(Sigmak); 621 | 622 | %Calcolo distanza tra posizione reale e stimata 623 | errore = sqrt((Xk-muk)'*(Xk-muk)); 624 | 625 | %Memorizzazione dati 626 | matrix_xk(i, :) = Xk(:, :)'; 627 | matrix_mu_pred(i, :) = mu_pred(:, :)'; 628 | matrix_muk(i, :) = muk(:, :)'; 629 | matrix_err(i, :) = errore; 630 | matrix_sigmak(:, :, i) = Sigmak; 631 | 632 | end 633 | 634 | %Calcolo dei limiti della finestra di visualizzazione 635 | x_min = min(matrix_xk(:, 1)); 636 | x_max = max(matrix_xk(:, 1)); 637 | y_min = min(matrix_xk(:, 2)); 638 | y_max = max(matrix_xk(:, 2)); 639 | ymuro = m*(x_min-50:0.2:x_max+50)+b; 640 | y_min_ass = min(y_min, min(ymuro)); 641 | y_max_ass = max(y_max, max(ymuro)); 642 | 643 | %Plot dei parametri 644 | x = x_min-20:0.2:x_max+20; 645 | set(gca, 'Color', [0.3 0.3 0.3]); 646 | lim_axis = [x_min-20 x_max+20 y_min_ass-20 y_max_ass+20]; 647 | axis(lim_axis); 648 | x_step = (lim_axis(2)-lim_axis(1))/5; 649 | y_step = (lim_axis(4)-lim_axis(3))/5; 650 | set(handles.Plot, 'XTick', round(100*(lim_axis(1):x_step:lim_axis(2)))/100); 651 | set(handles.Plot, 'YTick', round(100*(lim_axis(3):y_step:lim_axis(4)))/100); 652 | hold on 653 | plot(x, m*x+b, '.-', 'LineWidth', 9, 'Color', [1 0.5 0]); 654 | %plot(x, m*x+b, '-g', 'LineWidth', 3); 655 | plot(X0(1, 1), X0(2, 1), 'xc'); 656 | plot(mu0(1, 1), mu0(2, 1), 'or'); 657 | plot(matrix_muk(1, 1), matrix_muk(1, 2), 'xr'); 658 | plot(matrix_xk(:, 1), matrix_xk(:, 2), 'c'); 659 | h2 = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'ro', 'markersize', 5, 'markerfacecolor', 'r'); 660 | vcov = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'r'); 661 | 662 | %Rappresentazione puntuale dell'andamento stimato del robot 663 | fastdraw = true; 664 | if (fastdraw) 665 | drawstep = 2; %Parametro proporzionale alla velocita' di rappresentazione (>1) 666 | for i = 1+drawstep : drawstep-1 : size(matrix_muk, 1) 667 | h1 = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'r'); 668 | set(h2, 'XData', matrix_muk(i, 1), 'YData', matrix_muk(i, 2)); 669 | set(h1, 'XData', matrix_muk(i-drawstep:i, 1), 'YData', matrix_muk(i-drawstep:i, 2)); 670 | if (sum(sum(matrix_sigmak(:, :, i) == zeros(2))) ~= 4) 671 | pvcov = PlotEllipse(matrix_muk(i, :)', matrix_sigmak(:, :, i), cov_gain); 672 | set(vcov, 'xdata', pvcov(1, :), 'ydata', pvcov(2, :)); 673 | end 674 | drawnow; 675 | if (flag_stop) 676 | return 677 | end 678 | end 679 | else 680 | for i = 1 : size(matrix_muk, 1) 681 | h1 = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'r'); 682 | set(h2, 'XData', matrix_muk(i, 1), 'YData', matrix_muk(i, 2)); 683 | set(h1, 'XData', matrix_muk(i, 1), 'YData', matrix_muk(i, 2)); 684 | if (sum(sum(matrix_sigmak(:, :, i) == zeros(2))) ~= 4) 685 | pvcov = PlotEllipse(matrix_muk(i, :)', matrix_sigmak(:, :, i), cov_gain); 686 | set(vcov, 'xdata', pvcov(1, :), 'ydata', pvcov(2, :)); 687 | end 688 | drawnow; 689 | if (flag_stop) 690 | return 691 | end 692 | end 693 | end 694 | 695 | %h = legend('Muro', 'X0', 'mu0', 'mkinit', 'Xk', 'Robot', 'Mk', 'Location', 'Best'); 696 | 697 | function S = symmetric_m(S) 698 | % Funzione per mantenere la forma simmetrica di una matrice ed ovviare ad 699 | % eventuali errori di arrotondamento scaturiti dai calcoli 700 | S = (S+S')*0.5; 701 | 702 | function el = PlotEllipse(x, P, nSigma) 703 | % Funzione per il tracciamento di una singola ellisse di covarianza 704 | N = 8; 705 | step = 2*pi/N; 706 | if (all(diag(P))) 707 | [V, D] = eig(P); 708 | y = nSigma * [cos(0:step:2*pi); sin(0:step:2*pi)]; 709 | el = V*sqrtm(D)*y; 710 | el = [el el(:, 1)] + repmat(x, 1, size(el, 2)+1); 711 | el(:, end) = NaN; 712 | end 713 | -------------------------------------------------------------------------------- /03-KalmanFilter/KalmanF03.m: -------------------------------------------------------------------------------- 1 | function varargout = KalmanF03(varargin) 2 | % KALMANF03 MATLAB code for KalmanF03.fig 3 | % KALMANF03, by itself, creates a new KALMANF03 or raises the existing 4 | % singleton*. 5 | % 6 | % H = KALMANF03 returns the handle to a new KALMANF03 or the handle to 7 | % the existing singleton*. 8 | % 9 | % KALMANF03('CALLBACK', hObject, eventData, handles, ...) calls the local 10 | % function named CALLBACK in KALMANF03.M with the given input arguments. 11 | % 12 | % KALMANF03('Property', 'Value', ...) creates a new KALMANF03 or raises the 13 | % existing singleton*. Starting from the left, property value pairs are 14 | % applied to the GUI before KalmanF03_OpeningFcn gets called. An 15 | % unrecognized property name or invalid value makes property application 16 | % stop. All inputs are passed to KalmanF03_OpeningFcn via varargin. 17 | % 18 | % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one 19 | % instance to run (singleton)". 20 | % 21 | % See also: GUIDE, GUIDATA, GUIHANDLES 22 | % 23 | % -- 24 | % Salvatore La Bua (slabua@gmail.com) 25 | % GLB 26 | % MTM 27 | % DICGIM - University of Palermo 28 | 29 | % Edit the above text to modify the response to help KalmanF03 30 | 31 | % Last Modified by GUIDE v2.5 16-Oct-2013 01:51:28 32 | 33 | % Begin initialization code - DO NOT EDIT 34 | gui_Singleton = 1; 35 | gui_State = struct('gui_Name', mfilename, ... 36 | 'gui_Singleton', gui_Singleton, ... 37 | 'gui_OpeningFcn', @KalmanF03_OpeningFcn, ... 38 | 'gui_OutputFcn', @KalmanF03_OutputFcn, ... 39 | 'gui_LayoutFcn', [], ... 40 | 'gui_Callback', []); 41 | if nargin && ischar(varargin{1}) 42 | gui_State.gui_Callback = str2func(varargin{1}); 43 | end 44 | 45 | if nargout 46 | [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); 47 | else 48 | gui_mainfcn(gui_State, varargin{:}); 49 | end 50 | % End initialization code - DO NOT EDIT 51 | 52 | % --- Executes just before KalmanF03 is made visible. 53 | function KalmanF03_OpeningFcn(hObject, eventdata, handles, varargin) 54 | % This function has no output args, see OutputFcn. 55 | % hObject handle to figure 56 | % eventdata reserved - to be defined in a future version of MATLAB 57 | % handles structure with handles and user data (see GUIDATA) 58 | % varargin command line arguments to KalmanF03 (see VARARGIN) 59 | handles = initialize_gui(hObject, handles); 60 | % Choose default command line output for KalmanF03 61 | handles.output = hObject; 62 | 63 | % Update handles structure 64 | guidata(hObject, handles); 65 | 66 | % UIWAIT makes KalmanF03 wait for user response (see UIRESUME) 67 | % uiwait(handles.figure1); 68 | 69 | % --- Outputs from this function are returned to the command line. 70 | function varargout = KalmanF03_OutputFcn(hObject, eventdata, handles) 71 | % varargout cell array for returning output args (see VARARGOUT); 72 | % hObject handle to figure 73 | % eventdata reserved - to be defined in a future version of MATLAB 74 | % handles structure with handles and user data (see GUIDATA) 75 | 76 | % Get default command line output from handles structure 77 | varargout{1} = handles.output; 78 | 79 | % --- Executes on button press in Default. 80 | function Default_Callback(hObject, eventdata, handles) 81 | % hObject handle to Default (see GCBO) 82 | % eventdata reserved - to be defined in a future version of MATLAB 83 | % handles structure with handles and user data (see GUIDATA) 84 | flag_default = get(handles.Default, 'Value'); 85 | if (flag_default) 86 | set_Default(hObject, handles); 87 | disable_fields(hObject, handles); 88 | else 89 | set_Default(hObject, handles); 90 | enable_fields(hObject, handles); 91 | end 92 | % Hint: get(hObject, 'Value') returns toggle state of Default 93 | 94 | function Mu0_X_Callback(hObject, eventdata, handles) 95 | % hObject handle to Mu0_X (see GCBO) 96 | % eventdata reserved - to be defined in a future version of MATLAB 97 | % handles structure with handles and user data (see GUIDATA) 98 | 99 | % Hints: get(hObject, 'String') returns contents of Mu0_X as text 100 | % str2double(get(hObject, 'String')) returns contents of Mu0_X as a double 101 | 102 | % --- Executes during object creation, after setting all properties. 103 | function Mu0_X_CreateFcn(hObject, eventdata, handles) 104 | % hObject handle to Mu0_X (see GCBO) 105 | % eventdata reserved - to be defined in a future version of MATLAB 106 | % handles empty - handles not created until after all CreateFcns called 107 | 108 | % Hint: edit controls usually have a white background on Windows. 109 | % See ISPC and COMPUTER. 110 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 111 | set(hObject, 'BackgroundColor', 'white'); 112 | end 113 | 114 | function Mu0_Y_Callback(hObject, eventdata, handles) 115 | % hObject handle to Mu0_Y (see GCBO) 116 | % eventdata reserved - to be defined in a future version of MATLAB 117 | % handles structure with handles and user data (see GUIDATA) 118 | 119 | % Hints: get(hObject, 'String') returns contents of Mu0_Y as text 120 | % str2double(get(hObject, 'String')) returns contents of Mu0_Y as a double 121 | 122 | % --- Executes during object creation, after setting all properties. 123 | function Mu0_Y_CreateFcn(hObject, eventdata, handles) 124 | % hObject handle to Mu0_Y (see GCBO) 125 | % eventdata reserved - to be defined in a future version of MATLAB 126 | % handles empty - handles not created until after all CreateFcns called 127 | 128 | % Hint: edit controls usually have a white background on Windows. 129 | % See ISPC and COMPUTER. 130 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 131 | set(hObject, 'BackgroundColor', 'white'); 132 | end 133 | 134 | function X0_X_Callback(hObject, eventdata, handles) 135 | % hObject handle to X0_X (see GCBO) 136 | % eventdata reserved - to be defined in a future version of MATLAB 137 | % handles structure with handles and user data (see GUIDATA) 138 | 139 | % Hints: get(hObject, 'String') returns contents of X0_X as text 140 | % str2double(get(hObject, 'String')) returns contents of X0_X as a double 141 | 142 | % --- Executes during object creation, after setting all properties. 143 | function X0_X_CreateFcn(hObject, eventdata, handles) 144 | % hObject handle to X0_X (see GCBO) 145 | % eventdata reserved - to be defined in a future version of MATLAB 146 | % handles empty - handles not created until after all CreateFcns called 147 | 148 | % Hint: edit controls usually have a white background on Windows. 149 | % See ISPC and COMPUTER. 150 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 151 | set(hObject, 'BackgroundColor', 'white'); 152 | end 153 | 154 | function X0_Y_Callback(hObject, eventdata, handles) 155 | % hObject handle to X0_Y (see GCBO) 156 | % eventdata reserved - to be defined in a future version of MATLAB 157 | % handles structure with handles and user data (see GUIDATA) 158 | 159 | % Hints: get(hObject, 'String') returns contents of X0_Y as text 160 | % str2double(get(hObject, 'String')) returns contents of X0_Y as a double 161 | 162 | % --- Executes during object creation, after setting all properties. 163 | function X0_Y_CreateFcn(hObject, eventdata, handles) 164 | % hObject handle to X0_Y (see GCBO) 165 | % eventdata reserved - to be defined in a future version of MATLAB 166 | % handles empty - handles not created until after all CreateFcns called 167 | 168 | % Hint: edit controls usually have a white background on Windows. 169 | % See ISPC and COMPUTER. 170 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 171 | set(hObject, 'BackgroundColor', 'white'); 172 | end 173 | 174 | function Direction_Callback(hObject, eventdata, handles) 175 | % hObject handle to Direction (see GCBO) 176 | % eventdata reserved - to be defined in a future version of MATLAB 177 | % handles structure with handles and user data (see GUIDATA) 178 | 179 | % Hints: get(hObject, 'String') returns contents of Direction as text 180 | % str2double(get(hObject, 'String')) returns contents of Direction as a double 181 | 182 | % --- Executes during object creation, after setting all properties. 183 | function Direction_CreateFcn(hObject, eventdata, handles) 184 | % hObject handle to Direction (see GCBO) 185 | % eventdata reserved - to be defined in a future version of MATLAB 186 | % handles empty - handles not created until after all CreateFcns called 187 | 188 | % Hint: edit controls usually have a white background on Windows. 189 | % See ISPC and COMPUTER. 190 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 191 | set(hObject, 'BackgroundColor', 'white'); 192 | end 193 | 194 | function Control_noise_Callback(hObject, eventdata, handles) 195 | % hObject handle to Control_noise (see GCBO) 196 | % eventdata reserved - to be defined in a future version of MATLAB 197 | % handles structure with handles and user data (see GUIDATA) 198 | 199 | % Hints: get(hObject, 'String') returns contents of Control_noise as text 200 | % str2double(get(hObject, 'String')) returns contents of Control_noise as a double 201 | 202 | % --- Executes during object creation, after setting all properties. 203 | function Control_noise_CreateFcn(hObject, eventdata, handles) 204 | % hObject handle to Control_noise (see GCBO) 205 | % eventdata reserved - to be defined in a future version of MATLAB 206 | % handles empty - handles not created until after all CreateFcns called 207 | 208 | % Hint: edit controls usually have a white background on Windows. 209 | % See ISPC and COMPUTER. 210 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 211 | set(hObject, 'BackgroundColor', 'white'); 212 | end 213 | 214 | function Sensor_noise_Callback(hObject, eventdata, handles) 215 | % hObject handle to Sensor_noise (see GCBO) 216 | % eventdata reserved - to be defined in a future version of MATLAB 217 | % handles structure with handles and user data (see GUIDATA) 218 | 219 | % Hints: get(hObject, 'String') returns contents of Sensor_noise as text 220 | % str2double(get(hObject, 'String')) returns contents of Sensor_noise as a double 221 | 222 | % --- Executes during object creation, after setting all properties. 223 | function Sensor_noise_CreateFcn(hObject, eventdata, handles) 224 | % hObject handle to Sensor_noise (see GCBO) 225 | % eventdata reserved - to be defined in a future version of MATLAB 226 | % handles empty - handles not created until after all CreateFcns called 227 | 228 | % Hint: edit controls usually have a white background on Windows. 229 | % See ISPC and COMPUTER. 230 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 231 | set(hObject, 'BackgroundColor', 'white'); 232 | end 233 | 234 | function Sigma_stdX_Callback(hObject, eventdata, handles) 235 | % hObject handle to Sigma_std_Theta (see GCBO) 236 | % eventdata reserved - to be defined in a future version of MATLAB 237 | % handles structure with handles and user data (see GUIDATA) 238 | 239 | % Hints: get(hObject, 'String') returns contents of Sigma_std_Theta as text 240 | % str2double(get(hObject, 'String')) returns contents of Sigma_std_Theta as a double 241 | 242 | % --- Executes during object creation, after setting all properties. 243 | function Sigma_stdX_CreateFcn(hObject, eventdata, handles) 244 | % hObject handle to Sigma_std_Theta (see GCBO) 245 | % eventdata reserved - to be defined in a future version of MATLAB 246 | % handles empty - handles not created until after all CreateFcns called 247 | 248 | % Hint: edit controls usually have a white background on Windows. 249 | % See ISPC and COMPUTER. 250 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 251 | set(hObject, 'BackgroundColor', 'white'); 252 | end 253 | 254 | function Sigma_stdY_Callback(hObject, eventdata, handles) 255 | % hObject handle to Sigma_std_Theta (see GCBO) 256 | % eventdata reserved - to be defined in a future version of MATLAB 257 | % handles structure with handles and user data (see GUIDATA) 258 | 259 | % Hints: get(hObject, 'String') returns contents of Sigma_std_Theta as text 260 | % str2double(get(hObject, 'String')) returns contents of Sigma_std_Theta as a double 261 | 262 | % --- Executes during object creation, after setting all properties. 263 | function Sigma_stdY_CreateFcn(hObject, eventdata, handles) 264 | % hObject handle to Sigma_std_Theta (see GCBO) 265 | % eventdata reserved - to be defined in a future version of MATLAB 266 | % handles empty - handles not created until after all CreateFcns called 267 | 268 | % Hint: edit controls usually have a white background on Windows. 269 | % See ISPC and COMPUTER. 270 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 271 | set(hObject, 'BackgroundColor', 'white'); 272 | end 273 | 274 | function Sigma_std_Theta_Callback(hObject, eventdata, handles) 275 | % hObject handle to Sigma_stdY (see GCBO) 276 | % eventdata reserved - to be defined in a future version of MATLAB 277 | % handles structure with handles and user data (see GUIDATA) 278 | 279 | % Hints: get(hObject, 'String') returns contents of Sigma_stdY as text 280 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdY as a double 281 | 282 | % --- Executes during object creation, after setting all properties. 283 | function Sigma_std_Theta_CreateFcn(hObject, eventdata, handles) 284 | % hObject handle to Sigma_stdY (see GCBO) 285 | % eventdata reserved - to be defined in a future version of MATLAB 286 | % handles empty - handles not created until after all CreateFcns called 287 | 288 | % Hint: edit controls usually have a white background on Windows. 289 | % See ISPC and COMPUTER. 290 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 291 | set(hObject, 'BackgroundColor', 'white'); 292 | end 293 | 294 | function Linear_velocity_Callback(hObject, eventdata, handles) 295 | % hObject handle to Linear_velocity (see GCBO) 296 | % eventdata reserved - to be defined in a future version of MATLAB 297 | % handles structure with handles and user data (see GUIDATA) 298 | 299 | % Hints: get(hObject, 'String') returns contents of Linear_velocity as text 300 | % str2double(get(hObject, 'String')) returns contents of Linear_velocity as a double 301 | 302 | % --- Executes during object creation, after setting all properties. 303 | function Linear_velocity_CreateFcn(hObject, eventdata, handles) 304 | % hObject handle to Linear_velocity (see GCBO) 305 | % eventdata reserved - to be defined in a future version of MATLAB 306 | % handles empty - handles not created until after all CreateFcns called 307 | 308 | % Hint: edit controls usually have a white background on Windows. 309 | % See ISPC and COMPUTER. 310 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 311 | set(hObject, 'BackgroundColor', 'white'); 312 | end 313 | 314 | function Angular_velocity_Callback(hObject, eventdata, handles) 315 | % hObject handle to Angular_velocity (see GCBO) 316 | % eventdata reserved - to be defined in a future version of MATLAB 317 | % handles structure with handles and user data (see GUIDATA) 318 | 319 | % Hints: get(hObject, 'String') returns contents of Angular_velocity as text 320 | % str2double(get(hObject, 'String')) returns contents of Angular_velocity as a double 321 | 322 | % --- Executes during object creation, after setting all properties. 323 | function Angular_velocity_CreateFcn(hObject, eventdata, handles) 324 | % hObject handle to Angular_velocity (see GCBO) 325 | % eventdata reserved - to be defined in a future version of MATLAB 326 | % handles empty - handles not created until after all CreateFcns called 327 | 328 | % Hint: edit controls usually have a white background on Windows. 329 | % See ISPC and COMPUTER. 330 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 331 | set(hObject, 'BackgroundColor', 'white'); 332 | end 333 | 334 | function Frequency_Callback(hObject, eventdata, handles) 335 | % hObject handle to Frequency (see GCBO) 336 | % eventdata reserved - to be defined in a future version of MATLAB 337 | % handles structure with handles and user data (see GUIDATA) 338 | 339 | % Hints: get(hObject, 'String') returns contents of Frequency as text 340 | % str2double(get(hObject, 'String')) returns contents of Frequency as a double 341 | 342 | % --- Executes during object creation, after setting all properties. 343 | function Frequency_CreateFcn(hObject, eventdata, handles) 344 | % hObject handle to Frequency (see GCBO) 345 | % eventdata reserved - to be defined in a future version of MATLAB 346 | % handles empty - handles not created until after all CreateFcns called 347 | 348 | % Hint: edit controls usually have a white background on Windows. 349 | % See ISPC and COMPUTER. 350 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 351 | set(hObject, 'BackgroundColor', 'white'); 352 | end 353 | 354 | function Wall_parameters_m_Callback(hObject, eventdata, handles) 355 | % hObject handle to Wall_parameters_m (see GCBO) 356 | % eventdata reserved - to be defined in a future version of MATLAB 357 | % handles structure with handles and user data (see GUIDATA) 358 | 359 | % Hints: get(hObject, 'String') returns contents of Wall_parameters_m as text 360 | % str2double(get(hObject, 'String')) returns contents of Wall_parameters_m as a double 361 | 362 | % --- Executes during object creation, after setting all properties. 363 | function Wall_parameters_m_CreateFcn(hObject, eventdata, handles) 364 | % hObject handle to Wall_parameters_m (see GCBO) 365 | % eventdata reserved - to be defined in a future version of MATLAB 366 | % handles empty - handles not created until after all CreateFcns called 367 | 368 | % Hint: edit controls usually have a white background on Windows. 369 | % See ISPC and COMPUTER. 370 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 371 | set(hObject, 'BackgroundColor', 'white'); 372 | end 373 | 374 | function Wall_parameters_q_Callback(hObject, eventdata, handles) 375 | % hObject handle to Wall_parameters_q (see GCBO) 376 | % eventdata reserved - to be defined in a future version of MATLAB 377 | % handles structure with handles and user data (see GUIDATA) 378 | 379 | % Hints: get(hObject, 'String') returns contents of Wall_parameters_q as text 380 | % str2double(get(hObject, 'String')) returns contents of Wall_parameters_q as a double 381 | 382 | % --- Executes during object creation, after setting all properties. 383 | function Wall_parameters_q_CreateFcn(hObject, eventdata, handles) 384 | % hObject handle to Wall_parameters_q (see GCBO) 385 | % eventdata reserved - to be defined in a future version of MATLAB 386 | % handles empty - handles not created until after all CreateFcns called 387 | 388 | % Hint: edit controls usually have a white background on Windows. 389 | % See ISPC and COMPUTER. 390 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 391 | set(hObject, 'BackgroundColor', 'white'); 392 | end 393 | 394 | % --- Executes on button press in Play. 395 | function Play_Callback(hObject, eventdata, handles) 396 | % hObject handle to Play (see GCBO) 397 | % eventdata reserved - to be defined in a future version of MATLAB 398 | % handles structure with handles and user data (see GUIDATA) 399 | cla(handles.Plot, 'reset'); 400 | set(handles.Plot, 'Color', [0.3 0.3 0.3]); 401 | zoom off; 402 | pan off; 403 | set(handles.Default, 'Enable', 'OFF'); 404 | disable_fields(hObject, handles); 405 | set_Legend(hObject, handles); 406 | 407 | [mu0, X0, theta, r, q, Sigma0, vl, va, freq, m, b, iter] = create_var(hObject, handles); 408 | Esercitazione3(mu0, X0, theta, r, q, Sigma0, vl, va, freq, m, b, iter, hObject, handles); 409 | 410 | set(handles.Default, 'Enable', 'ON'); 411 | flag_default = get(handles.Default, 'Value'); 412 | if (~flag_default) 413 | enable_fields(hObject, handles); 414 | end 415 | 416 | function set_Legend(hObject, handles) 417 | im = imread('legend3.png'); 418 | set(handles.Legend, 'HandleVisibility', 'ON'); 419 | axes(handles.Legend); 420 | image(im); 421 | set(handles.Legend, 'XTick', []); 422 | set(handles.Legend, 'YTick', []); 423 | 424 | function steps_Callback(hObject, eventdata, handles) 425 | % hObject handle to steps (see GCBO) 426 | % eventdata reserved - to be defined in a future version of MATLAB 427 | % handles structure with handles and user data (see GUIDATA) 428 | 429 | % Hints: get(hObject, 'String') returns contents of steps as text 430 | % str2double(get(hObject, 'String')) returns contents of steps as a double 431 | 432 | % --- Executes during object creation, after setting all properties. 433 | function steps_CreateFcn(hObject, eventdata, handles) 434 | % hObject handle to steps (see GCBO) 435 | % eventdata reserved - to be defined in a future version of MATLAB 436 | % handles empty - handles not created until after all CreateFcns called 437 | 438 | % Hint: edit controls usually have a white background on Windows. 439 | % See ISPC and COMPUTER. 440 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 441 | set(hObject, 'BackgroundColor', 'white'); 442 | end 443 | 444 | % --- Executes on button press in Stop. 445 | function Stop_Callback(hObject, eventdata, handles) 446 | % hObject handle to Stop (see GCBO) 447 | % eventdata reserved - to be defined in a future version of MATLAB 448 | % handles structure with handles and user data (see GUIDATA) 449 | global flag_stop; 450 | flag_stop = 1; 451 | set(handles.Default, 'Enable', 'ON'); 452 | flag_default = get(handles.Default, 'Value'); 453 | if (~flag_default) 454 | enable_fields(hObject, handles); 455 | end 456 | 457 | % --- Executes on button press in Reset. 458 | function Reset_Callback(hObject, eventdata, handles) 459 | % hObject handle to Reset (see GCBO) 460 | % eventdata reserved - to be defined in a future version of MATLAB 461 | % handles structure with handles and user data (see GUIDATA) 462 | Stop_Callback(hObject, eventdata, handles); 463 | zoom off; 464 | pan off; 465 | h = initialize_gui(hObject, handles); 466 | set(handles.Legend, 'XTick', []); 467 | set(handles.Legend, 'YTick', []); 468 | 469 | % --- Executes on button press in Zoom_in. 470 | function Zoom_in_Callback(hObject, eventdata, handles) 471 | % hObject handle to Zoom_in (see GCBO) 472 | % eventdata reserved - to be defined in a future version of MATLAB 473 | % handles structure with handles and user data (see GUIDATA) 474 | 475 | % --- Executes on button press in Zoom_out. 476 | function Zoom_out_Callback(hObject, eventdata, handles) 477 | % hObject handle to Zoom_out (see GCBO) 478 | % eventdata reserved - to be defined in a future version of MATLAB 479 | % handles structure with handles and user data (see GUIDATA) 480 | 481 | % --- Executes on button press in Zoom. 482 | function Zoom_Callback(hObject, eventdata, handles) 483 | % hObject handle to Zoom (see GCBO) 484 | % eventdata reserved - to be defined in a future version of MATLAB 485 | % handles structure with handles and user data (see GUIDATA) 486 | zoom on; 487 | 488 | % --- Executes on button press in Pan. 489 | function Pan_Callback(hObject, eventdata, handles) 490 | % hObject handle to Pan (see GCBO) 491 | % eventdata reserved - to be defined in a future version of MATLAB 492 | % handles structure with handles and user data (see GUIDATA) 493 | pan on; 494 | 495 | function handles = initialize_gui(hObject, handles) 496 | % If the metricdata field is present and the reset flag is false, it means 497 | % we are we are just re-initializing a GUI by calling it from the cmd line 498 | % while it is up. So, bail out as we dont want to reset the data. 499 | cla(handles.Plot, 'reset'); 500 | set(handles.Plot, 'Color', [0.3 0.3 0.3]); 501 | cla(handles.Legend, 'reset'); 502 | set(handles.Legend, 'Color', [0.3 0.3 0.3]); 503 | set(handles.Legend, 'Visible', 'OFF'); 504 | set(handles.Legend, 'XTick', []); 505 | set(handles.Legend, 'YTick', []); 506 | set(handles.Default, 'Enable', 'ON'); 507 | set(handles.Default, 'Value', 1); 508 | set_Default(hObject, handles); 509 | disable_fields(hObject, handles); 510 | 511 | function set_Default(hObject, handles) 512 | set(handles.Mu0_X, 'String', 7); 513 | set(handles.Mu0_Y, 'String', 5); 514 | set(handles.X0_X, 'String', -1); 515 | set(handles.X0_Y, 'String', 0); 516 | set(handles.Direction, 'String', -0.52); 517 | set(handles.Control_noise, 'String', 0); 518 | set(handles.Sensor_noise, 'String', 0.33); 519 | set(handles.Sigma_stdX, 'String', 100); 520 | set(handles.Sigma_stdY, 'String', 100); 521 | set(handles.Sigma_std_Theta, 'String', 0.76); 522 | set(handles.Linear_velocity, 'String', 4.5); 523 | set(handles.Angular_velocity, 'String', 0.1); 524 | set(handles.Frequency, 'String', 50); 525 | set(handles.Wall_parameters_m, 'String', 0); 526 | set(handles.Wall_parameters_q, 'String', 2); 527 | set(handles.steps, 'String', 500); 528 | 529 | function disable_fields(hObject, handles) 530 | set(handles.Mu0_X, 'Enable', 'OFF'); 531 | set(handles.Mu0_Y, 'Enable', 'OFF'); 532 | set(handles.X0_X, 'Enable', 'OFF'); 533 | set(handles.X0_Y, 'Enable', 'OFF'); 534 | set(handles.Direction, 'Enable', 'OFF'); 535 | set(handles.Control_noise, 'Enable', 'OFF'); 536 | set(handles.Sensor_noise, 'Enable', 'OFF'); 537 | set(handles.Sigma_stdX, 'Enable', 'OFF'); 538 | set(handles.Sigma_stdY, 'Enable', 'OFF'); 539 | set(handles.Sigma_std_Theta, 'Enable', 'OFF'); 540 | set(handles.Linear_velocity, 'Enable', 'OFF'); 541 | set(handles.Angular_velocity, 'Enable', 'OFF'); 542 | set(handles.Frequency, 'Enable', 'OFF'); 543 | set(handles.Wall_parameters_m, 'Enable', 'OFF'); 544 | set(handles.Wall_parameters_q, 'Enable', 'OFF'); 545 | set(handles.steps, 'Enable', 'OFF'); 546 | 547 | function enable_fields(hObject, handles) 548 | set(handles.Mu0_X, 'Enable', 'ON'); 549 | set(handles.Mu0_Y, 'Enable', 'ON'); 550 | set(handles.X0_X, 'Enable', 'ON'); 551 | set(handles.X0_Y, 'Enable', 'ON'); 552 | set(handles.Direction, 'Enable', 'ON'); 553 | set(handles.Control_noise, 'Enable', 'ON'); 554 | set(handles.Sensor_noise, 'Enable', 'ON'); 555 | set(handles.Sigma_stdX, 'Enable', 'ON'); 556 | set(handles.Sigma_stdY, 'Enable', 'ON'); 557 | set(handles.Sigma_std_Theta, 'Enable', 'ON'); 558 | set(handles.Linear_velocity, 'Enable', 'ON'); 559 | set(handles.Angular_velocity, 'Enable', 'ON'); 560 | set(handles.Frequency, 'Enable', 'ON'); 561 | set(handles.Wall_parameters_m, 'Enable', 'ON'); 562 | set(handles.Wall_parameters_q, 'Enable', 'ON'); 563 | set(handles.steps, 'Enable', 'ON'); 564 | 565 | function [mu0, X0, theta, r, q, Sigma0, vl, va, freq, m, b, iter] = create_var(hObject, handles) 566 | Mu0_X = eval(get(handles.Mu0_X, 'String')); 567 | Mu0_Y = eval(get(handles.Mu0_Y, 'String')); 568 | X0_X = eval(get(handles.X0_X, 'String')); 569 | X0_Y = eval(get(handles.X0_Y, 'String')); 570 | Direction = eval(get(handles.Direction, 'String')); 571 | Control_noise = eval(get(handles.Control_noise, 'String')); 572 | Sensor_noise = eval(get(handles.Sensor_noise, 'String')); 573 | Sigma_stdX = eval(get(handles.Sigma_stdX, 'String')); 574 | Sigma_stdY = eval(get(handles.Sigma_stdY, 'String')); 575 | Sigma_std = eval(get(handles.Sigma_std_Theta, 'String')); 576 | Linear_velocity = eval(get(handles.Linear_velocity, 'String')); 577 | Angular_velocity = eval(get(handles.Angular_velocity, 'String')); 578 | Frequency = eval(get(handles.Frequency, 'String')); 579 | Wall_parameters_m = eval(get(handles.Wall_parameters_m, 'String')); 580 | Wall_parameters_q = eval(get(handles.Wall_parameters_q, 'String')); 581 | steps = eval(get(handles.steps, 'String')); 582 | 583 | mu0 = [Mu0_X; Mu0_Y; 0]; 584 | theta = Direction; 585 | X0 = [X0_X; X0_Y; theta]; 586 | r = Control_noise; 587 | if (Sensor_noise <= 2e-5) 588 | Sensor_noise = 2e-5; 589 | end 590 | q = Sensor_noise^2; 591 | Sigma0 = [Sigma_stdX, 0, 0; 0, Sigma_stdY, 0; 0, 0, Sigma_std]; 592 | vl = Linear_velocity; 593 | va = Angular_velocity; 594 | freq = Frequency; 595 | m = Wall_parameters_m; 596 | b = Wall_parameters_q; 597 | iter = steps; 598 | 599 | function Esercitazione3(mu0, X0, theta, r, q, Sigma0, vl, va, freq, m, b, iter, hObject, handles) 600 | %La Funzione implementa il Filtro di Kalman esteso in presenza del muro e 601 | %angolo di sterzata 602 | 603 | global flag_stop; 604 | flag_stop = 0; 605 | axes(handles.Plot); 606 | 607 | cov_gain = 1; 608 | dt = 1/freq; %Periodo di campionamento 609 | U = [vl; vl; va]; %Vettore di controllo sul robot 610 | I = eye(3); %Matrice identita' utilizzata nel filtro di Kalman esteso 611 | 612 | A = [1 0 0; 0 1 0; 0 0 1]; 613 | B = [cos(theta)*dt 0 0; 0 sin(theta)*dt 0; 0 0 dt]; 614 | C = [-2*m/(sqrt(m*m+1)), 2/(sqrt(m*m+1)) 0]; 615 | 616 | R = r*eye(3); %matrice di covarianza del processo 617 | 618 | %Inizializzazione algoritmo 619 | Xk = X0; 620 | Sigmak = Sigma0; 621 | muk = mu0; 622 | matrix_err = zeros(iter, 1); 623 | matrix_mu_pred = zeros(iter, 2); 624 | matrix_xk = zeros(iter, 2); 625 | matrix_muk = zeros(iter, 2); 626 | matrix_sigmak = zeros(2, 2, iter); 627 | 628 | for i = 1 : iter 629 | 630 | %distanza tra la posizione reale del robot e il muro 631 | dist_PR = (-m*Xk(1, 1) + Xk(2, 1) - b) / sqrt( (-m)^2 +1 ); 632 | 633 | %Matrice di covarianza della misura 634 | %Il rumore generato e' a media 0 e varianza pari a q 635 | Q = (0 + sqrt(q) .* randn(1)); 636 | Xk = (A*Xk)+(B*U); %Posizione reale del robot 637 | Z_temp = 2*dist_PR + Q; %Misura della distanza con aggiunta di rumore 638 | Zk = Z_temp - 2*(-b/sqrt(1+m*m)); %Misura 639 | 640 | G = [1, 0, -U(1, 1)*sin(theta)*dt; 0 1 U(2, 1)*cos(theta)*dt; 0 0 1]; %Jacobiano 641 | 642 | %Fase di Predizione 643 | mu_pred(1, 1) = muk(1, 1) + U(1, 1)*cos(muk(3, 1))*dt; 644 | mu_pred(2, 1) = muk(2, 1) + U(2, 1)*sin(muk(3, 1))*dt; 645 | mu_pred(3, 1) = muk(3, 1) + U(3, 1)*dt; 646 | 647 | Sigma_pred = G*(Sigmak*G') + R; 648 | 649 | %Fase di Correzione 650 | S = symmetric_m(C*(Sigma_pred*C') + q); 651 | %K_gain = Sigma_pred*(C'*inv(S)); %Guadagno di Kalman 652 | K_gain = Sigma_pred*(C'/S); %Guadagno di Kalman 653 | muk = mu_pred+K_gain*(Zk-C*mu_pred); %Posizione stimata del robot 654 | Sigmak = (I-K_gain*C)*Sigma_pred; %Matrice di covarianza stimata del robot 655 | Sigmak = symmetric_m(Sigmak); 656 | 657 | %Calcolo distanza tra posizione reale e stimata 658 | errore = sqrt((Xk-muk)'*(Xk-muk)); 659 | 660 | %Memorizzazione dati 661 | matrix_xk(i, 1) = Xk(1, 1); 662 | matrix_xk(i, 2) = Xk(2, 1); 663 | matrix_mu_pred(i, 1) = mu_pred(1, 1); 664 | matrix_mu_pred(i, 2) = mu_pred(2, 1); 665 | matrix_muk(i, 1) = muk(1, 1); 666 | matrix_muk(i, 2) = muk(2, 1); 667 | matrix_err(i, :) = errore; 668 | matrix_sigmak(:, :, i) = Sigmak(1:2, 1:2); 669 | 670 | end 671 | 672 | %Calcolo dei limiti della finestra di visualizzazione 673 | x_min = min(matrix_xk(:, 1)); 674 | x_max = max(matrix_xk(:, 1)); 675 | y_min = min(matrix_xk(:, 2)); 676 | y_max = max(matrix_xk(:, 2)); 677 | ymuro = m*(x_min-50:0.2:x_max+50)+b; 678 | y_min_ass = min(y_min, min(ymuro)); 679 | y_max_ass = max(y_max, max(ymuro)); 680 | 681 | %Plot dei parametri 682 | x = x_min-20:0.2:x_max+20; 683 | set(gca, 'Color', [0.3 0.3 0.3]); 684 | lim_axis = [x_min-20 x_max+20 y_min_ass-20 y_max_ass+20]; 685 | axis(lim_axis); 686 | x_step = (lim_axis(2)-lim_axis(1))/5; 687 | y_step = (lim_axis(4)-lim_axis(3))/5; 688 | set(handles.Plot, 'XTick', round(100*(lim_axis(1):x_step:lim_axis(2)))/100); 689 | set(handles.Plot, 'YTick', round(100*(lim_axis(3):y_step:lim_axis(4)))/100); 690 | hold on 691 | plot(x, m*x+b, '.-', 'LineWidth', 9, 'Color', [1 0.5 0]); 692 | %plot(x, m*x+b, '--k', 'LineWidth', 3); 693 | plot(X0(1, 1), X0(2, 1), 'xc'); 694 | plot(mu0(1, 1), mu0(2, 1), 'or'); 695 | plot(matrix_muk(1, 1), matrix_muk(1, 2), 'xr'); 696 | plot(matrix_xk(:, 1), matrix_xk(:, 2), 'c'); 697 | h2 = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'ro', 'markersize', 5, 'markerfacecolor', 'r'); 698 | vcov = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'r'); 699 | 700 | %Rappresentazione puntuale dell'andamento stimato del robot 701 | fastdraw = true; 702 | if (fastdraw) 703 | drawstep = 2; %Parametro proporzionale alla velocita' di rappresentazione (>1) 704 | for i = 1+drawstep : drawstep-1 : size(matrix_muk, 1) 705 | h1 = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'r'); 706 | set(h2, 'XData', matrix_muk(i, 1), 'YData', matrix_muk(i, 2)); 707 | set(h1, 'XData', matrix_muk(i-drawstep:i, 1), 'YData', matrix_muk(i-drawstep:i, 2)); 708 | if (sum(sum(matrix_sigmak(:, :, i) == zeros(2))) ~= 4) 709 | pvcov = PlotEllipse(matrix_muk(i, :)', matrix_sigmak(:, :, i), cov_gain); 710 | set(vcov, 'xdata', pvcov(1, :), 'ydata', pvcov(2, :)); 711 | end 712 | drawnow; 713 | if (flag_stop) 714 | return 715 | end 716 | end 717 | else 718 | for i = 1 : size(matrix_muk, 1) 719 | h1 = plot(matrix_muk(1, 1), matrix_muk(1, 2), 'r'); 720 | set(h2, 'XData', matrix_muk(i, 1), 'YData', matrix_muk(i, 2)); 721 | set(h1, 'XData', matrix_muk(i, 1), 'YData', matrix_muk(i, 2)); 722 | if (sum(sum(matrix_sigmak(:, :, i) == zeros(2))) ~= 4) 723 | pvcov = PlotEllipse(matrix_muk(i, :)', matrix_sigmak(:, :, i), cov_gain); 724 | set(vcov, 'xdata', pvcov(1, :), 'ydata', pvcov(2, :)); 725 | end 726 | drawnow; 727 | if (flag_stop) 728 | return 729 | end 730 | end 731 | end 732 | 733 | %h = legend('Muro', 'X0', 'mu0', 'mkinit', 'Xk', 'Robot', 'Mk', 'Location', 'Best'); 734 | 735 | function S = symmetric_m(S) 736 | % Funzione per mantenere la forma simmetrica di una matrice ed ovviare ad 737 | % eventuali errori di arrotondamento scaturiti dai calcoli 738 | S = (S+S')*0.5; 739 | 740 | function el = PlotEllipse(x, P, nSigma) 741 | % Funzione per il tracciamento di una singola ellisse di covarianza 742 | N = 8; 743 | step = 2*pi/N; 744 | if (all(diag(P))) 745 | [V, D] = eig(P); 746 | y = nSigma * [cos(0:step:2*pi); sin(0:step:2*pi)]; 747 | el = V*sqrtm(D)*y; 748 | el = [el el(:, 1)] + repmat(x, 1, size(el, 2)+1); 749 | el(:, end) = NaN; 750 | end 751 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU 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 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /04-ParticleFilter/ParticleF04.m: -------------------------------------------------------------------------------- 1 | function varargout = ParticleF04(varargin) 2 | % PARTICLEF04 MATLAB code for ParticleF04.fig 3 | % PARTICLEF04, by itself, creates a new PARTICLEF04 or raises the existing 4 | % singleton*. 5 | % 6 | % H = PARTICLEF04 returns the handle to a new PARTICLEF04 or the handle to 7 | % the existing singleton*. 8 | % 9 | % PARTICLEF04('CALLBACK', hObject, eventData, handles, ...) calls the local 10 | % function named CALLBACK in PARTICLEF04.M with the given input arguments. 11 | % 12 | % PARTICLEF04('Property', 'Value', ...) creates a new PARTICLEF04 or raises the 13 | % existing singleton*. Starting from the left, property value pairs are 14 | % applied to the GUI before ParticleF04_OpeningFcn gets called. An 15 | % unrecognized property name or invalid value makes property application 16 | % stop. All inputs are passed to ParticleF04_OpeningFcn via varargin. 17 | % 18 | % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one 19 | % instance to run (singleton)". 20 | % 21 | % See also: GUIDE, GUIDATA, GUIHANDLES 22 | % 23 | % -- 24 | % Salvatore La Bua (slabua@gmail.com) 25 | % GLB 26 | % MTM 27 | % DICGIM - University of Palermo 28 | 29 | % Edit the above text to modify the response to help ParticleF04 30 | 31 | % Last Modified by GUIDE v2.5 16-Oct-2013 01:52:45 32 | 33 | % Begin initialization code - DO NOT EDIT 34 | gui_Singleton = 1; 35 | gui_State = struct('gui_Name', mfilename, ... 36 | 'gui_Singleton', gui_Singleton, ... 37 | 'gui_OpeningFcn', @ParticleF04_OpeningFcn, ... 38 | 'gui_OutputFcn', @ParticleF04_OutputFcn, ... 39 | 'gui_LayoutFcn', [], ... 40 | 'gui_Callback', []); 41 | if nargin && ischar(varargin{1}) 42 | gui_State.gui_Callback = str2func(varargin{1}); 43 | end 44 | 45 | if nargout 46 | [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); 47 | else 48 | gui_mainfcn(gui_State, varargin{:}); 49 | end 50 | % End initialization code - DO NOT EDIT 51 | 52 | % --- Executes just before ParticleF04 is made visible. 53 | function ParticleF04_OpeningFcn(hObject, eventdata, handles, varargin) 54 | % This function has no output args, see OutputFcn. 55 | % hObject handle to figure 56 | % eventdata reserved - to be defined in a future version of MATLAB 57 | % handles structure with handles and user data (see GUIDATA) 58 | % varargin command line arguments to ParticleF04 (see VARARGIN) 59 | handles = initialize_gui(hObject, handles); 60 | % Choose default command line output for ParticleF04 61 | handles.output = hObject; 62 | 63 | % Update handles structure 64 | guidata(hObject, handles); 65 | 66 | % UIWAIT makes ParticleF04 wait for user response (see UIRESUME) 67 | % uiwait(handles.figure1); 68 | 69 | % --- Outputs from this function are returned to the command line. 70 | function varargout = ParticleF04_OutputFcn(hObject, eventdata, handles) 71 | % varargout cell array for returning output args (see VARARGOUT); 72 | % hObject handle to figure 73 | % eventdata reserved - to be defined in a future version of MATLAB 74 | % handles structure with handles and user data (see GUIDATA) 75 | 76 | % Get default command line output from handles structure 77 | varargout{1} = handles.output; 78 | 79 | % --- Executes on button press in Default. 80 | function Default_Callback(hObject, eventdata, handles) 81 | % hObject handle to Default (see GCBO) 82 | % eventdata reserved - to be defined in a future version of MATLAB 83 | % handles structure with handles and user data (see GUIDATA) 84 | flag_default = get(handles.Default, 'Value'); 85 | if (flag_default) 86 | set_Default(hObject, handles); 87 | disable_fields(hObject, handles); 88 | else 89 | set_Default(hObject, handles); 90 | enable_fields(hObject, handles); 91 | end 92 | % Hint: get(hObject, 'Value') returns toggle state of Default 93 | 94 | function Mu0_X_Callback(hObject, eventdata, handles) 95 | % hObject handle to Mu0_X (see GCBO) 96 | % eventdata reserved - to be defined in a future version of MATLAB 97 | % handles structure with handles and user data (see GUIDATA) 98 | 99 | % Hints: get(hObject, 'String') returns contents of Mu0_X as text 100 | % str2double(get(hObject, 'String')) returns contents of Mu0_X as a double 101 | 102 | % --- Executes during object creation, after setting all properties. 103 | function Mu0_X_CreateFcn(hObject, eventdata, handles) 104 | % hObject handle to Mu0_X (see GCBO) 105 | % eventdata reserved - to be defined in a future version of MATLAB 106 | % handles empty - handles not created until after all CreateFcns called 107 | 108 | % Hint: edit controls usually have a white background on Windows. 109 | % See ISPC and COMPUTER. 110 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 111 | set(hObject, 'BackgroundColor', 'white'); 112 | end 113 | 114 | function Mu0_Y_Callback(hObject, eventdata, handles) 115 | % hObject handle to Mu0_Y (see GCBO) 116 | % eventdata reserved - to be defined in a future version of MATLAB 117 | % handles structure with handles and user data (see GUIDATA) 118 | 119 | % Hints: get(hObject, 'String') returns contents of Mu0_Y as text 120 | % str2double(get(hObject, 'String')) returns contents of Mu0_Y as a double 121 | 122 | % --- Executes during object creation, after setting all properties. 123 | function Mu0_Y_CreateFcn(hObject, eventdata, handles) 124 | % hObject handle to Mu0_Y (see GCBO) 125 | % eventdata reserved - to be defined in a future version of MATLAB 126 | % handles empty - handles not created until after all CreateFcns called 127 | 128 | % Hint: edit controls usually have a white background on Windows. 129 | % See ISPC and COMPUTER. 130 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 131 | set(hObject, 'BackgroundColor', 'white'); 132 | end 133 | 134 | function X0_X_Callback(hObject, eventdata, handles) 135 | % hObject handle to X0_X (see GCBO) 136 | % eventdata reserved - to be defined in a future version of MATLAB 137 | % handles structure with handles and user data (see GUIDATA) 138 | 139 | % Hints: get(hObject, 'String') returns contents of X0_X as text 140 | % str2double(get(hObject, 'String')) returns contents of X0_X as a double 141 | 142 | % --- Executes during object creation, after setting all properties. 143 | function X0_X_CreateFcn(hObject, eventdata, handles) 144 | % hObject handle to X0_X (see GCBO) 145 | % eventdata reserved - to be defined in a future version of MATLAB 146 | % handles empty - handles not created until after all CreateFcns called 147 | 148 | % Hint: edit controls usually have a white background on Windows. 149 | % See ISPC and COMPUTER. 150 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 151 | set(hObject, 'BackgroundColor', 'white'); 152 | end 153 | 154 | function X0_Y_Callback(hObject, eventdata, handles) 155 | % hObject handle to X0_Y (see GCBO) 156 | % eventdata reserved - to be defined in a future version of MATLAB 157 | % handles structure with handles and user data (see GUIDATA) 158 | 159 | % Hints: get(hObject, 'String') returns contents of X0_Y as text 160 | % str2double(get(hObject, 'String')) returns contents of X0_Y as a double 161 | 162 | % --- Executes during object creation, after setting all properties. 163 | function X0_Y_CreateFcn(hObject, eventdata, handles) 164 | % hObject handle to X0_Y (see GCBO) 165 | % eventdata reserved - to be defined in a future version of MATLAB 166 | % handles empty - handles not created until after all CreateFcns called 167 | 168 | % Hint: edit controls usually have a white background on Windows. 169 | % See ISPC and COMPUTER. 170 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 171 | set(hObject, 'BackgroundColor', 'white'); 172 | end 173 | 174 | function Direction_Callback(hObject, eventdata, handles) 175 | % hObject handle to Direction (see GCBO) 176 | % eventdata reserved - to be defined in a future version of MATLAB 177 | % handles structure with handles and user data (see GUIDATA) 178 | 179 | % Hints: get(hObject, 'String') returns contents of Direction as text 180 | % str2double(get(hObject, 'String')) returns contents of Direction as a double 181 | 182 | % --- Executes during object creation, after setting all properties. 183 | function Direction_CreateFcn(hObject, eventdata, handles) 184 | % hObject handle to Direction (see GCBO) 185 | % eventdata reserved - to be defined in a future version of MATLAB 186 | % handles empty - handles not created until after all CreateFcns called 187 | 188 | % Hint: edit controls usually have a white background on Windows. 189 | % See ISPC and COMPUTER. 190 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 191 | set(hObject, 'BackgroundColor', 'white'); 192 | end 193 | 194 | function Control_noise_Callback(hObject, eventdata, handles) 195 | % hObject handle to Control_noise (see GCBO) 196 | % eventdata reserved - to be defined in a future version of MATLAB 197 | % handles structure with handles and user data (see GUIDATA) 198 | 199 | % Hints: get(hObject, 'String') returns contents of Control_noise as text 200 | % str2double(get(hObject, 'String')) returns contents of Control_noise as a double 201 | 202 | % --- Executes during object creation, after setting all properties. 203 | function Control_noise_CreateFcn(hObject, eventdata, handles) 204 | % hObject handle to Control_noise (see GCBO) 205 | % eventdata reserved - to be defined in a future version of MATLAB 206 | % handles empty - handles not created until after all CreateFcns called 207 | 208 | % Hint: edit controls usually have a white background on Windows. 209 | % See ISPC and COMPUTER. 210 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 211 | set(hObject, 'BackgroundColor', 'white'); 212 | end 213 | 214 | function Sensor_noise_Callback(hObject, eventdata, handles) 215 | % hObject handle to Sensor_noise (see GCBO) 216 | % eventdata reserved - to be defined in a future version of MATLAB 217 | % handles structure with handles and user data (see GUIDATA) 218 | 219 | % Hints: get(hObject, 'String') returns contents of Sensor_noise as text 220 | % str2double(get(hObject, 'String')) returns contents of Sensor_noise as a double 221 | 222 | % --- Executes during object creation, after setting all properties. 223 | function Sensor_noise_CreateFcn(hObject, eventdata, handles) 224 | % hObject handle to Sensor_noise (see GCBO) 225 | % eventdata reserved - to be defined in a future version of MATLAB 226 | % handles empty - handles not created until after all CreateFcns called 227 | 228 | % Hint: edit controls usually have a white background on Windows. 229 | % See ISPC and COMPUTER. 230 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 231 | set(hObject, 'BackgroundColor', 'white'); 232 | end 233 | 234 | function Sigma_stdX_Callback(hObject, eventdata, handles) 235 | % hObject handle to Sigma_stdX (see GCBO) 236 | % eventdata reserved - to be defined in a future version of MATLAB 237 | % handles structure with handles and user data (see GUIDATA) 238 | 239 | % Hints: get(hObject, 'String') returns contents of Sigma_stdX as text 240 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdX as a double 241 | 242 | % --- Executes during object creation, after setting all properties. 243 | function Sigma_stdX_CreateFcn(hObject, eventdata, handles) 244 | % hObject handle to Sigma_stdX (see GCBO) 245 | % eventdata reserved - to be defined in a future version of MATLAB 246 | % handles empty - handles not created until after all CreateFcns called 247 | 248 | % Hint: edit controls usually have a white background on Windows. 249 | % See ISPC and COMPUTER. 250 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 251 | set(hObject, 'BackgroundColor', 'white'); 252 | end 253 | 254 | function Sigma_stdY_Callback(hObject, eventdata, handles) 255 | % hObject handle to Sigma_stdY (see GCBO) 256 | % eventdata reserved - to be defined in a future version of MATLAB 257 | % handles structure with handles and user data (see GUIDATA) 258 | 259 | % Hints: get(hObject, 'String') returns contents of Sigma_stdY as text 260 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdY as a double 261 | 262 | % --- Executes during object creation, after setting all properties. 263 | function Sigma_stdY_CreateFcn(hObject, eventdata, handles) 264 | % hObject handle to Sigma_stdY (see GCBO) 265 | % eventdata reserved - to be defined in a future version of MATLAB 266 | % handles empty - handles not created until after all CreateFcns called 267 | 268 | % Hint: edit controls usually have a white background on Windows. 269 | % See ISPC and COMPUTER. 270 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 271 | set(hObject, 'BackgroundColor', 'white'); 272 | end 273 | 274 | function Sigma_std_Theta_Callback(hObject, eventdata, handles) 275 | % hObject handle to Sigma_stdY (see GCBO) 276 | % eventdata reserved - to be defined in a future version of MATLAB 277 | % handles structure with handles and user data (see GUIDATA) 278 | 279 | % Hints: get(hObject, 'String') returns contents of Sigma_stdY as text 280 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdY as a double 281 | 282 | % --- Executes during object creation, after setting all properties. 283 | function Sigma_std_Theta_CreateFcn(hObject, eventdata, handles) 284 | % hObject handle to Sigma_stdY (see GCBO) 285 | % eventdata reserved - to be defined in a future version of MATLAB 286 | % handles empty - handles not created until after all CreateFcns called 287 | 288 | % Hint: edit controls usually have a white background on Windows. 289 | % See ISPC and COMPUTER. 290 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 291 | set(hObject, 'BackgroundColor', 'white'); 292 | end 293 | 294 | function Linear_velocity_Callback(hObject, eventdata, handles) 295 | % hObject handle to Linear_velocity (see GCBO) 296 | % eventdata reserved - to be defined in a future version of MATLAB 297 | % handles structure with handles and user data (see GUIDATA) 298 | 299 | % Hints: get(hObject, 'String') returns contents of Linear_velocity as text 300 | % str2double(get(hObject, 'String')) returns contents of Linear_velocity as a double 301 | 302 | % --- Executes during object creation, after setting all properties. 303 | function Linear_velocity_CreateFcn(hObject, eventdata, handles) 304 | % hObject handle to Linear_velocity (see GCBO) 305 | % eventdata reserved - to be defined in a future version of MATLAB 306 | % handles empty - handles not created until after all CreateFcns called 307 | 308 | % Hint: edit controls usually have a white background on Windows. 309 | % See ISPC and COMPUTER. 310 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 311 | set(hObject, 'BackgroundColor', 'white'); 312 | end 313 | 314 | function Frequency_Callback(hObject, eventdata, handles) 315 | % hObject handle to Frequency (see GCBO) 316 | % eventdata reserved - to be defined in a future version of MATLAB 317 | % handles structure with handles and user data (see GUIDATA) 318 | 319 | % Hints: get(hObject, 'String') returns contents of Frequency as text 320 | % str2double(get(hObject, 'String')) returns contents of Frequency as a double 321 | 322 | % --- Executes during object creation, after setting all properties. 323 | function Frequency_CreateFcn(hObject, eventdata, handles) 324 | % hObject handle to Frequency (see GCBO) 325 | % eventdata reserved - to be defined in a future version of MATLAB 326 | % handles empty - handles not created until after all CreateFcns called 327 | 328 | % Hint: edit controls usually have a white background on Windows. 329 | % See ISPC and COMPUTER. 330 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 331 | set(hObject, 'BackgroundColor', 'white'); 332 | end 333 | 334 | % --- Executes on button press in Play. 335 | function Play_Callback(hObject, eventdata, handles) 336 | % hObject handle to Play (see GCBO) 337 | % eventdata reserved - to be defined in a future version of MATLAB 338 | % handles structure with handles and user data (see GUIDATA) 339 | cla(handles.Plot, 'reset'); 340 | set(handles.Plot, 'Color', [0.3 0.3 0.3]); 341 | zoom off; 342 | pan off; 343 | set(handles.Default, 'Enable', 'OFF'); 344 | disable_fields(hObject, handles); 345 | set_Legend(hObject, handles); 346 | 347 | [campioni, g_std, NL, r, q, Sigma0, vl, theta, freq, iter] = create_var(hObject, handles); 348 | Esercitazione4(campioni, g_std, NL, r, q, Sigma0, vl, theta, freq, iter, hObject, handles); 349 | 350 | set(handles.Default, 'Enable', 'ON'); 351 | flag_default = get(handles.Default, 'Value'); 352 | if (~flag_default) 353 | enable_fields(hObject, handles); 354 | end 355 | 356 | function set_Legend(hObject, handles) 357 | im = imread('legend4.png'); 358 | set(handles.Legend, 'HandleVisibility', 'ON'); 359 | axes(handles.Legend); 360 | image(im); 361 | set(handles.Legend, 'XTick', []); 362 | set(handles.Legend, 'YTick', []); 363 | 364 | function steps_Callback(hObject, eventdata, handles) 365 | % hObject handle to steps (see GCBO) 366 | % eventdata reserved - to be defined in a future version of MATLAB 367 | % handles structure with handles and user data (see GUIDATA) 368 | 369 | % Hints: get(hObject, 'String') returns contents of steps as text 370 | % str2double(get(hObject, 'String')) returns contents of steps as a double 371 | 372 | % --- Executes during object creation, after setting all properties. 373 | function steps_CreateFcn(hObject, eventdata, handles) 374 | % hObject handle to steps (see GCBO) 375 | % eventdata reserved - to be defined in a future version of MATLAB 376 | % handles empty - handles not created until after all CreateFcns called 377 | 378 | % Hint: edit controls usually have a white background on Windows. 379 | % See ISPC and COMPUTER. 380 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 381 | set(hObject, 'BackgroundColor', 'white'); 382 | end 383 | 384 | % --- Executes on button press in Stop. 385 | function Stop_Callback(hObject, eventdata, handles) 386 | % hObject handle to Stop (see GCBO) 387 | % eventdata reserved - to be defined in a future version of MATLAB 388 | % handles structure with handles and user data (see GUIDATA) 389 | global flag_stop; 390 | flag_stop = 1; 391 | set(handles.Default, 'Enable', 'ON'); 392 | flag_default = get(handles.Default, 'Value'); 393 | if (~flag_default) 394 | enable_fields(hObject, handles); 395 | end 396 | 397 | % --- Executes on button press in Reset. 398 | function Reset_Callback(hObject, eventdata, handles) 399 | % hObject handle to Reset (see GCBO) 400 | % eventdata reserved - to be defined in a future version of MATLAB 401 | % handles structure with handles and user data (see GUIDATA) 402 | Stop_Callback(hObject, eventdata, handles); 403 | zoom off; 404 | pan off; 405 | h = initialize_gui(hObject, handles); 406 | set(handles.Legend, 'XTick', []); 407 | set(handles.Legend, 'YTick', []); 408 | 409 | % --- Executes on button press in Zoom. 410 | function Zoom_Callback(hObject, eventdata, handles) 411 | % hObject handle to Zoom (see GCBO) 412 | % eventdata reserved - to be defined in a future version of MATLAB 413 | % handles structure with handles and user data (see GUIDATA) 414 | zoom on; 415 | 416 | % --- Executes on button press in Pan. 417 | function Pan_Callback(hObject, eventdata, handles) 418 | % hObject handle to Pan (see GCBO) 419 | % eventdata reserved - to be defined in a future version of MATLAB 420 | % handles structure with handles and user data (see GUIDATA) 421 | pan on; 422 | 423 | function NL_min_Callback(hObject, eventdata, handles) 424 | % hObject handle to NL_min (see GCBO) 425 | % eventdata reserved - to be defined in a future version of MATLAB 426 | % handles structure with handles and user data (see GUIDATA) 427 | 428 | % Hints: get(hObject, 'String') returns contents of NL_min as text 429 | % str2double(get(hObject, 'String')) returns contents of NL_min as a double 430 | 431 | % --- Executes during object creation, after setting all properties. 432 | function NL_min_CreateFcn(hObject, eventdata, handles) 433 | % hObject handle to NL_min (see GCBO) 434 | % eventdata reserved - to be defined in a future version of MATLAB 435 | % handles empty - handles not created until after all CreateFcns called 436 | 437 | % Hint: edit controls usually have a white background on Windows. 438 | % See ISPC and COMPUTER. 439 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 440 | set(hObject, 'BackgroundColor', 'white'); 441 | end 442 | 443 | function NL_fixed_Callback(hObject, eventdata, handles) 444 | % hObject handle to NL_fixed (see GCBO) 445 | % eventdata reserved - to be defined in a future version of MATLAB 446 | % handles structure with handles and user data (see GUIDATA) 447 | 448 | % Hints: get(hObject, 'String') returns contents of NL_fixed as text 449 | % str2double(get(hObject, 'String')) returns contents of NL_fixed as a double 450 | 451 | % --- Executes during object creation, after setting all properties. 452 | function NL_fixed_CreateFcn(hObject, eventdata, handles) 453 | % hObject handle to NL_fixed (see GCBO) 454 | % eventdata reserved - to be defined in a future version of MATLAB 455 | % handles empty - handles not created until after all CreateFcns called 456 | 457 | % Hint: edit controls usually have a white background on Windows. 458 | % See ISPC and COMPUTER. 459 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 460 | set(hObject, 'BackgroundColor', 'white'); 461 | end 462 | 463 | function NL_max_Callback(hObject, eventdata, handles) 464 | % hObject handle to NL_max (see GCBO) 465 | % eventdata reserved - to be defined in a future version of MATLAB 466 | % handles structure with handles and user data (see GUIDATA) 467 | 468 | % Hints: get(hObject, 'String') returns contents of NL_max as text 469 | % str2double(get(hObject, 'String')) returns contents of NL_max as a double 470 | 471 | % --- Executes during object creation, after setting all properties. 472 | function NL_max_CreateFcn(hObject, eventdata, handles) 473 | % hObject handle to NL_max (see GCBO) 474 | % eventdata reserved - to be defined in a future version of MATLAB 475 | % handles empty - handles not created until after all CreateFcns called 476 | 477 | % Hint: edit controls usually have a white background on Windows. 478 | % See ISPC and COMPUTER. 479 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 480 | set(hObject, 'BackgroundColor', 'white'); 481 | end 482 | 483 | function N_Samples_Callback(hObject, eventdata, handles) 484 | % hObject handle to N_Samples (see GCBO) 485 | % eventdata reserved - to be defined in a future version of MATLAB 486 | % handles structure with handles and user data (see GUIDATA) 487 | 488 | % Hints: get(hObject, 'String') returns contents of N_Samples as text 489 | % str2double(get(hObject, 'String')) returns contents of N_Samples as a double 490 | 491 | % --- Executes during object creation, after setting all properties. 492 | function N_Samples_CreateFcn(hObject, eventdata, handles) 493 | % hObject handle to N_Samples (see GCBO) 494 | % eventdata reserved - to be defined in a future version of MATLAB 495 | % handles empty - handles not created until after all CreateFcns called 496 | 497 | % Hint: edit controls usually have a white background on Windows. 498 | % See ISPC and COMPUTER. 499 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 500 | set(hObject, 'BackgroundColor', 'white'); 501 | end 502 | 503 | function Gaussian_std_Callback(hObject, eventdata, handles) 504 | % hObject handle to Gaussian_std (see GCBO) 505 | % eventdata reserved - to be defined in a future version of MATLAB 506 | % handles structure with handles and user data (see GUIDATA) 507 | 508 | % Hints: get(hObject, 'String') returns contents of Gaussian_std as text 509 | % str2double(get(hObject, 'String')) returns contents of Gaussian_std as a double 510 | 511 | % --- Executes during object creation, after setting all properties. 512 | function Gaussian_std_CreateFcn(hObject, eventdata, handles) 513 | % hObject handle to Gaussian_std (see GCBO) 514 | % eventdata reserved - to be defined in a future version of MATLAB 515 | % handles empty - handles not created until after all CreateFcns called 516 | 517 | % Hint: edit controls usually have a white background on Windows. 518 | % See ISPC and COMPUTER. 519 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 520 | set(hObject, 'BackgroundColor', 'white'); 521 | end 522 | 523 | function handles = initialize_gui(hObject, handles) 524 | % If the metricdata field is present and the reset flag is false, it means 525 | % we are we are just re-initializing a GUI by calling it from the cmd line 526 | % while it is up. So, bail out as we dont want to reset the data. 527 | set(handles.Sigma_std_Theta, 'Visible', 'OFF'); 528 | cla(handles.Plot, 'reset'); 529 | set(handles.Plot, 'Color', [0.3 0.3 0.3]); 530 | cla(handles.Legend, 'reset'); 531 | set(handles.Legend, 'Color', [0.3 0.3 0.3]); 532 | set(handles.Legend, 'Visible', 'OFF'); 533 | set(handles.Legend, 'XTick', []); 534 | set(handles.Legend, 'YTick', []); 535 | set(handles.Default, 'Enable', 'ON'); 536 | set(handles.Default, 'Value', 1); 537 | set_Default(hObject, handles); 538 | disable_fields(hObject, handles); 539 | 540 | function set_Default(hObject, handles) 541 | set(handles.N_Samples, 'String', 1500); 542 | set(handles.Uniform, 'Value', 1); 543 | set(handles.Gaussian, 'Value', 0); 544 | set(handles.Gaussian_std, 'String', 10); 545 | set(handles.Fixed, 'Value', 1); 546 | set(handles.Between, 'Value', 0); 547 | set(handles.NL_fixed, 'String', 20); 548 | set(handles.NL_min, 'String', 20); 549 | set(handles.NL_max, 'String', 40); 550 | set(handles.Control_noise, 'String', 0.1); 551 | set(handles.Sensor_noise, 'String', 5); 552 | set(handles.Sigma_stdX, 'String', 100); 553 | set(handles.Sigma_stdY, 'String', 100); 554 | set(handles.Sigma_std_Theta, 'String', 0.76); 555 | set(handles.Linear_velocity, 'String', 4.5); 556 | set(handles.Direction, 'String', -0.52); 557 | set(handles.Frequency, 'String', 50); 558 | set(handles.steps, 'String', 500); 559 | 560 | function disable_fields(hObject, handles) 561 | set(handles.N_Samples, 'Enable', 'OFF'); 562 | set(handles.Uniform, 'Enable', 'OFF'); 563 | set(handles.Gaussian, 'Enable', 'OFF'); 564 | set(handles.Gaussian_std, 'Enable', 'OFF'); 565 | set(handles.Fixed, 'Enable', 'OFF'); 566 | set(handles.Between, 'Enable', 'OFF'); 567 | set(handles.NL_fixed, 'Enable', 'OFF'); 568 | set(handles.NL_min, 'Enable', 'OFF'); 569 | set(handles.NL_max, 'Enable', 'OFF'); 570 | set(handles.Control_noise, 'Enable', 'OFF'); 571 | set(handles.Sensor_noise, 'Enable', 'OFF'); 572 | set(handles.Sigma_stdX, 'Enable', 'OFF'); 573 | set(handles.Sigma_stdY, 'Enable', 'OFF'); 574 | set(handles.Sigma_std_Theta, 'Enable', 'OFF'); 575 | set(handles.Linear_velocity, 'Enable', 'OFF'); 576 | set(handles.Direction, 'Enable', 'OFF'); 577 | set(handles.Frequency, 'Enable', 'OFF'); 578 | set(handles.steps, 'Enable', 'OFF'); 579 | 580 | function enable_fields(hObject, handles) 581 | set(handles.N_Samples, 'Enable', 'ON'); 582 | set(handles.Uniform, 'Enable', 'ON'); 583 | set(handles.Gaussian, 'Enable', 'ON'); 584 | set(handles.Gaussian_std, 'Enable', 'ON'); 585 | set(handles.Fixed, 'Enable', 'ON'); 586 | set(handles.Between, 'Enable', 'ON'); 587 | set(handles.NL_fixed, 'Enable', 'ON'); 588 | set(handles.NL_min, 'Enable', 'ON'); 589 | set(handles.NL_max, 'Enable', 'ON'); 590 | set(handles.Control_noise, 'Enable', 'ON'); 591 | set(handles.Sensor_noise, 'Enable', 'ON'); 592 | set(handles.Sigma_stdX, 'Enable', 'ON'); 593 | set(handles.Sigma_stdY, 'Enable', 'ON'); 594 | set(handles.Sigma_std_Theta, 'Enable', 'ON'); 595 | set(handles.Linear_velocity, 'Enable', 'ON'); 596 | set(handles.Direction, 'Enable', 'ON'); 597 | set(handles.Frequency, 'Enable', 'ON'); 598 | set(handles.steps, 'Enable', 'ON'); 599 | 600 | function [campioni, g_std, NL, r, q, Sigma0, vl, theta, freq, iter] = create_var(hObject, handles) 601 | N_Samples = eval(get(handles.N_Samples, 'String')); 602 | Gaussian_std = eval(get(handles.Gaussian_std, 'String')); 603 | NL_fixed = eval(get(handles.NL_fixed, 'String')); 604 | NL_min = eval(get(handles.NL_min, 'String')); 605 | NL_max = eval(get(handles.NL_max, 'String')); 606 | Control_noise = eval(get(handles.Control_noise, 'String')); 607 | Sensor_noise = eval(get(handles.Sensor_noise, 'String')); 608 | Sigma_stdX = eval(get(handles.Sigma_stdX, 'String')); 609 | Sigma_stdY = eval(get(handles.Sigma_stdY, 'String')); 610 | Sigma_std = eval(get(handles.Sigma_std_Theta, 'String')); 611 | Linear_velocity = eval(get(handles.Linear_velocity, 'String')); 612 | Direction = eval(get(handles.Direction, 'String')); 613 | Frequency = eval(get(handles.Frequency, 'String')); 614 | steps = eval(get(handles.steps, 'String')); 615 | 616 | campioni = N_Samples; 617 | g_std = Gaussian_std; 618 | if (get(handles.Fixed, 'Value')) 619 | NL = NL_fixed; 620 | else 621 | NL = randi(NL_max-NL_min+1) + (NL_min-1); 622 | end 623 | r = Control_noise; 624 | q = Sensor_noise^2; 625 | Sigma0 = [Sigma_stdX, 0; 0, Sigma_stdY]; 626 | vl = Linear_velocity; 627 | theta = Direction; 628 | freq = Frequency; 629 | iter = steps; 630 | 631 | function Esercitazione4(campioni, g_std, NL, r, q, Sigma0, vl, theta, freq, iter, hObject, handles) 632 | %La Funzione implementa il Filtro Particellare 633 | 634 | global flag_stop; 635 | flag_stop = 0; 636 | axes(handles.Plot); 637 | 638 | dt = 1/freq; %Periodo di campionamento 639 | delta_s = [cos(theta)*vl*dt sin(theta)*vl*dt]; %Spostamento 640 | 641 | global map_dim; 642 | map_dim = [100 100]; 643 | X0 = map_dim / 2; %Posizione iniziale del robot 644 | 645 | L = zeros(NL, 2); 646 | 647 | misura = zeros(NL, 1); 648 | %Generazione dei landmark 649 | for i = 1 : NL 650 | L(i, 1) = map_dim(1)*rand; 651 | L(i, 2) = map_dim(2)*rand; 652 | end 653 | 654 | S = zeros(campioni, 2); 655 | global perc_uniform_samples; 656 | %Percentuale di campioni uniformemente distribuiti (kidnapped robot problem) 657 | perc_uniform_samples = 5; 658 | %Generazione delle particelle 659 | for i = 1 : campioni 660 | if (get(handles.Uniform, 'Value')) 661 | S(i, 1) = map_dim(1)*rand; %distribuzione di particelle uniforme 662 | S(i, 2) = map_dim(2)*rand; %distribuzione di particelle uniforme 663 | else 664 | S(i, 1) = X0(1)+g_std*randn(1); %distribuzione di particelle gaussiana 665 | S(i, 2) = X0(2)+g_std*randn(1); %centrata su X0 e deviazione standard g_std 666 | end 667 | end 668 | 669 | % Plot dei landmark e delle particelle iniziali 670 | hold on; 671 | set(gca, 'Color', [0.3 0.3 0.3]); 672 | plot(L(:, 1), L(:, 2), 'oc', 'markersize', 5); 673 | plot(L(:, 1), L(:, 2), 'oc', 'markersize', 5); 674 | plot(S(:, 1), S(:, 2), 'xr', 'markersize', 5); 675 | hold off; 676 | pause(2); 677 | 678 | %Inizializzazione dell'algoritmo 679 | Xk = X0; 680 | S_new = S; 681 | matrix_xk = []; 682 | 683 | for i = 1 : iter 684 | 685 | R = (0+sqrt(r).*randn(1)); %Generazione del rumore sul controllo 686 | 687 | %Applicazione del modello del moto 688 | Xk = motion_model(Xk, delta_s); %Posizione reale con eventuale aggiunta di rumore 689 | [S_new, index, index_corr] = particle_filter(Xk+R, S_new, L, delta_s, Sigma0, r, q); %Nuovo insieme di particelle 690 | 691 | %Plot dei parametri 692 | if ~mod(i, 10) %Aggiorno il grafico ogni 10 passi 693 | matrix_xk = [matrix_xk; Xk]; 694 | plot(L(:, 1), L(:, 2), 'oc', 'markersize', 5); %Plot dei landmark 695 | hold on; 696 | set(gca, 'Color', [0.3 0.3 0.3]); 697 | plot(S_new(:, 1), S_new(:, 2), 'xr'); %Plot delle particelle 698 | plot(Xk(1), Xk(2), 'ok', 'markersize', 7, 'MarkerFaceColor', 'k'); %Plot della posizione reale del robot 699 | plot(matrix_xk(:, 1), matrix_xk(:, 2), 'r.-'); 700 | plot([Xk(1), L(index, 1)], [Xk(2), L(index, 2)], 'g.-'); 701 | plot([Xk(1), L(index_corr, 1)], [Xk(2), L(index_corr, 2)], 'y.-'); 702 | pause(1/2); 703 | hold off 704 | if (flag_stop) 705 | return 706 | end 707 | end 708 | 709 | end 710 | 711 | function S_pred_new = darwin_law(Sigma0, S_pred, pesi) 712 | %Funzione che effettua un campionamento per importanza delle particelle rispetto ai loro pesi 713 | %Parametri IN: 714 | %Sigma0 : matrice di covarianza 715 | %S_pred : stima delle particelle 716 | %pesi : vettore dei pesi delle particelle 717 | %Parametri OUT: 718 | %S_pred_new : nuova stima delle particelle 719 | 720 | n = 1; %percentuale di particelle da campionare 721 | campioni = size(S_pred, 1); 722 | perc_sample = n*(floor(campioni/100))+1; 723 | 724 | R = chol(Sigma0); 725 | 726 | %Campionamento per importanza delle particelle con peso minore 727 | for i = 1 : perc_sample 728 | [c, j] = min(pesi); 729 | pesi(j) = 1; 730 | S_pred(j, :) = repmat(mean(S_pred), 1, 1)+ randn(1, 2)*R; 731 | end 732 | 733 | S_pred_new = S_pred; 734 | 735 | function X_new = motion_model(X, delta_s) 736 | %Applicazione del modello del moto 737 | %Parametri IN: 738 | %X : posizione del robot 739 | %delta_s : spostamento del robot 740 | %Parametri OUT: 741 | %X_new : nuova posizione del robot 742 | 743 | X_new = X + ones(size(X(:, 1)))*delta_s; 744 | % X_new = X + ones(size(X(:, 1)))*delta_s + (sqrt(0).*randn(size(X))); 745 | 746 | function [S_new, index, index_corr] = particle_filter(Xk, S, L, delta_s, Sigma0, r, q) 747 | %Filtro particellare 748 | %Parametri IN: 749 | %Xk : posizione reale del robot 750 | %S : insieme delle particelle 751 | %L : insieme dei landmark 752 | %delta_s : vettore spostamento 753 | %Sigma0 : matrice di covarianza 754 | %q : incertezza sulla misura 755 | %Parametri OUT: 756 | %S_new : insieme delle particelle ricampionate 757 | 758 | %Applicazione del modello del moto alle particelle 759 | % S_pred = motion_model(S, delta_s); 760 | S_pred = motion_model(S, delta_s) + (sqrt(r).*randn(size(S))); 761 | 762 | %Applicazione del modello della misura alle particelle 763 | [pesi, index, index_corr] = sensor_model(S_pred, Xk, L, q); 764 | 765 | % %Fase di campionamento 766 | % S_pred_new = darwin_law(Sigma0, S_pred, pesi); 767 | % 768 | % %Applicazione del modello della misura alle particelle campionate 769 | % pesi_new = sensor_model(S_pred_new, Xk, L, q); 770 | % 771 | % %Fase di ricampionamento 772 | % S_new = resampling(S_pred_new, pesi_new); 773 | 774 | S_new = wheel_resample(S_pred, pesi); 775 | 776 | function S_resample = resampling(S, pesi) 777 | %Funzione che effettua un ricampionamento delle particelle 778 | %Parametri IN: 779 | %S : insieme delle particelle 780 | %pesi : pesi delle particelle 781 | %Parametri OUT: 782 | %S_resample : insieme delle particelle campionate 783 | 784 | [n, m] = size(S); 785 | soglia = 0.00000001; 786 | 787 | S_resample = zeros(n, m); 788 | 789 | C = zeros(n, 1); 790 | 791 | C = cumsum(pesi); %Somma cumulativa 792 | 793 | i = 1; 794 | 795 | %Metodo Montecarlo per il ricampionamento 796 | for j = 1:n 797 | u = j/n; 798 | while u-C(i)>soglia %Per evitare errori dovuti agli arrotondamenti 799 | i = i+1; 800 | end 801 | 802 | S_resample(j, :) = S(i, :); 803 | end 804 | 805 | function [pesi, index, index_corr] = sensor_model(S, X, L, q) 806 | %La funzione applica il modello della misura 807 | %Parametri IN: 808 | %S : insieme delle particelle 809 | %X : posizione del robot 810 | %L : insieme dei landmark 811 | %q : incertezza sulla misura 812 | %Parametri OUT: 813 | %[pesi index] : vettore dei pesi delle particelle 814 | 815 | NL = size(L, 1); %Numero dei landmark 816 | campioni = size(S, 1); %Numero delle particelle 817 | 818 | misura = zeros(NL, 1); 819 | 820 | %Calcolo delle distanze tra il robot e tutti i landmark con aggiunta del 821 | %rumore 822 | for i = 1 : NL 823 | 824 | misura_corr(i) = 2*sqrt((X-L(i, :))*(X-L(i, :))'); 825 | misura(i) = misura_corr(i) + sqrt(q)*randn(1)/2; 826 | 827 | end 828 | 829 | [mis_min, index] = min(misura); %Calcolo del landmark stimato piu' vicino 830 | [mis_min, index_corr] = min(misura_corr); %Calcolo del landmark piu' vicino 831 | 832 | distanza = zeros(campioni, 1); 833 | 834 | %Calcolo della distanza tra i landmark e le particelle e dell'errore 835 | %commesso 836 | for i = 1 : campioni 837 | distanza(i) = sqrt((L(index, :)-S(i, :))*(L(index, :)-S(i, :))'); 838 | errore(i) = abs(mis_min-distanza(i)); 839 | end 840 | 841 | pesi_errori = 1./errore; %Calcolo dei pesi delle particelle 842 | pesi = pesi_errori./sum(pesi_errori); %Normalizzazione dei pesi 843 | 844 | function S_resample = wheel_resample(S, pesi) 845 | %Funzione che implementa il ricampionamento secondo il metodo della 846 | %roulette, simile al sistematic resampling 847 | %Parametri IN: 848 | %S : insieme di particelle da ricampionare 849 | %pesi : pesi delle particelle da ricampionare 850 | %Parametri OUT: 851 | %S_resample : insieme delle particelle ricampionate 852 | 853 | S_resample = []; 854 | N = length(pesi); 855 | 856 | global perc_uniform_samples; 857 | excluded = round(N / 100 * perc_uniform_samples); 858 | 859 | index = randi(N); 860 | beta = 0; 861 | mw = max(pesi); 862 | 863 | for i = 1 : N - excluded 864 | beta = beta + rand()*2*mw; 865 | 866 | while (beta > pesi(index)) 867 | beta = beta - pesi(index); 868 | index = index+1; 869 | if (index > N), index = 1; end; 870 | end 871 | 872 | S_resample = [S_resample; S(index, :)]; 873 | % S_resample = [S_resample; S(index, :)+(randn(1,2))*0.5]; 874 | 875 | end 876 | 877 | global map_dim; 878 | S_unif = generate_uniform_samples(excluded, map_dim); 879 | S_resample = [S_resample; S_unif]; 880 | 881 | function S = generate_uniform_samples(perc_samples, map_dim) 882 | %La funzione genera una percentuale di particelle uniformemente distribuite 883 | %per favorire la convergenza dell'algoritmo in caso di robot rapito 884 | %Parametri IN: 885 | %perc_samples : percentuale di particelle uniformemente distribuite 886 | %map_dim : dominio di generazione 887 | %Parametri OUT: 888 | %S : insieme di particelle generate 889 | for i = 1 : perc_samples 890 | S(i, 1) = map_dim(1)*rand; %distribuzione di particelle uniforme 891 | S(i, 2) = map_dim(2)*rand; %distribuzione di particelle uniforme 892 | end 893 | -------------------------------------------------------------------------------- /05-EKFSLAM/EKFSLAM05.m: -------------------------------------------------------------------------------- 1 | function varargout = EKFSLAM05(varargin) 2 | % EKFSLAM05 MATLAB code for EKFSLAM05.fig 3 | % EKFSLAM05, by itself, creates a new EKFSLAM05 or raises the existing 4 | % singleton*. 5 | % 6 | % H = EKFSLAM05 returns the handle to a new EKFSLAM05 or the handle to 7 | % the existing singleton*. 8 | % 9 | % EKFSLAM05('CALLBACK', hObject, eventData, handles, ...) calls the local 10 | % function named CALLBACK in EKFSLAM05.M with the given input arguments. 11 | % 12 | % EKFSLAM05('Property', 'Value', ...) creates a new EKFSLAM05 or raises the 13 | % existing singleton*. Starting from the left, property value pairs are 14 | % applied to the GUI before EKFSLAM05_OpeningFcn gets called. An 15 | % unrecognized property name or invalid value makes property application 16 | % stop. All inputs are passed to EKFSLAM05_OpeningFcn via varargin. 17 | % 18 | % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one 19 | % instance to run (singleton)". 20 | % 21 | % See also: GUIDE, GUIDATA, GUIHANDLES 22 | % 23 | % -- 24 | % Salvatore La Bua (slabua@gmail.com) 25 | % GLB 26 | % MTM 27 | % DICGIM - University of Palermo 28 | 29 | % Edit the above text to modify the response to help EKFSLAM05 30 | 31 | % Last Modified by GUIDE v2.5 16-Oct-2013 02:13:26 32 | 33 | % Begin initialization code - DO NOT EDIT 34 | gui_Singleton = 1; 35 | gui_State = struct('gui_Name', mfilename, ... 36 | 'gui_Singleton', gui_Singleton, ... 37 | 'gui_OpeningFcn', @EKFSLAM05_OpeningFcn, ... 38 | 'gui_OutputFcn', @EKFSLAM05_OutputFcn, ... 39 | 'gui_LayoutFcn', [], ... 40 | 'gui_Callback', []); 41 | if nargin && ischar(varargin{1}) 42 | gui_State.gui_Callback = str2func(varargin{1}); 43 | end 44 | 45 | if nargout 46 | [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); 47 | else 48 | gui_mainfcn(gui_State, varargin{:}); 49 | end 50 | % End initialization code - DO NOT EDIT 51 | 52 | % --- Executes just before EKFSLAM05 is made visible. 53 | function EKFSLAM05_OpeningFcn(hObject, eventdata, handles, varargin) 54 | % This function has no output args, see OutputFcn. 55 | % hObject handle to figure 56 | % eventdata reserved - to be defined in a future version of MATLAB 57 | % handles structure with handles and user data (see GUIDATA) 58 | % varargin command line arguments to EKFSLAM05 (see VARARGIN) 59 | handles = initialize_gui(hObject, handles); 60 | % Choose default command line output for EKFSLAM05 61 | handles.output = hObject; 62 | 63 | % Update handles structure 64 | guidata(hObject, handles); 65 | 66 | % UIWAIT makes EKFSLAM05 wait for user response (see UIRESUME) 67 | % uiwait(handles.figure1); 68 | 69 | % --- Outputs from this function are returned to the command line. 70 | function varargout = EKFSLAM05_OutputFcn(hObject, eventdata, handles) 71 | % varargout cell array for returning output args (see VARARGOUT); 72 | % hObject handle to figure 73 | % eventdata reserved - to be defined in a future version of MATLAB 74 | % handles structure with handles and user data (see GUIDATA) 75 | 76 | % Get default command line output from handles structure 77 | varargout{1} = handles.output; 78 | 79 | % --- Executes on button press in Default. 80 | function Default_Callback(hObject, eventdata, handles) 81 | % hObject handle to Default (see GCBO) 82 | % eventdata reserved - to be defined in a future version of MATLAB 83 | % handles structure with handles and user data (see GUIDATA) 84 | flag_default = get(handles.Default, 'Value'); 85 | if (flag_default) 86 | set_Default(hObject, handles); 87 | disable_fields(hObject, handles); 88 | else 89 | set_Default(hObject, handles); 90 | enable_fields(hObject, handles); 91 | end 92 | % Hint: get(hObject, 'Value') returns toggle state of Default 93 | 94 | % --- Executes on button press in Reset. 95 | function Reset_Callback(hObject, eventdata, handles) 96 | % hObject handle to Reset (see GCBO) 97 | % eventdata reserved - to be defined in a future version of MATLAB 98 | % handles structure with handles and user data (see GUIDATA) 99 | Stop_Callback(hObject, eventdata, handles); 100 | zoom off; 101 | pan off; 102 | h = initialize_gui(hObject, handles); 103 | set(handles.Legend, 'XTick', []); 104 | set(handles.Legend, 'YTick', []); 105 | 106 | % --- Executes on button press in Play. 107 | function Play_Callback(hObject, eventdata, handles) 108 | % hObject handle to Play (see GCBO) 109 | % eventdata reserved - to be defined in a future version of MATLAB 110 | % handles structure with handles and user data (see GUIDATA) 111 | cla(handles.Plot, 'reset'); 112 | %set(handles.Plot, 'Color', [0.3 0.3 0.3]); 113 | zoom off; 114 | pan off; 115 | set(handles.Default, 'Enable', 'OFF'); 116 | disable_fields(hObject, handles); 117 | set_Legend(hObject, handles); 118 | 119 | [X0, Mu0, vl, freq, R, Q, lm_vis, Sigma0, LOOPS, landmarks, waypoints] = create_var(hObject, handles); 120 | Esercitazione5(X0, Mu0, vl, freq, R, Q, lm_vis, Sigma0, LOOPS, landmarks, waypoints, hObject, handles); 121 | 122 | set(handles.Default, 'Enable', 'ON'); 123 | flag_default = get(handles.Default, 'Value'); 124 | if (~flag_default) 125 | enable_fields(hObject, handles); 126 | end 127 | 128 | % --- Executes on button press in Stop. 129 | function Stop_Callback(hObject, eventdata, handles) 130 | % hObject handle to Stop (see GCBO) 131 | % eventdata reserved - to be defined in a future version of MATLAB 132 | % handles structure with handles and user data (see GUIDATA) 133 | global flag_stop; 134 | flag_stop = 1; 135 | set(handles.Default, 'Enable', 'ON'); 136 | flag_default = get(handles.Default, 'Value'); 137 | if (~flag_default) 138 | enable_fields(hObject, handles); 139 | end 140 | 141 | function loops_Callback(hObject, eventdata, handles) 142 | % hObject handle to loops (see GCBO) 143 | % eventdata reserved - to be defined in a future version of MATLAB 144 | % handles structure with handles and user data (see GUIDATA) 145 | 146 | % Hints: get(hObject, 'String') returns contents of loops as text 147 | % str2double(get(hObject, 'String')) returns contents of loops as a double 148 | 149 | % --- Executes during object creation, after setting all properties. 150 | function loops_CreateFcn(hObject, eventdata, handles) 151 | % hObject handle to loops (see GCBO) 152 | % eventdata reserved - to be defined in a future version of MATLAB 153 | % handles empty - handles not created until after all CreateFcns called 154 | 155 | % Hint: edit controls usually have a white background on Windows. 156 | % See ISPC and COMPUTER. 157 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 158 | set(hObject, 'BackgroundColor', 'white'); 159 | end 160 | 161 | function Sigma_stdX_Callback(hObject, eventdata, handles) 162 | % hObject handle to Sigma_stdX (see GCBO) 163 | % eventdata reserved - to be defined in a future version of MATLAB 164 | % handles structure with handles and user data (see GUIDATA) 165 | 166 | % Hints: get(hObject, 'String') returns contents of Sigma_stdX as text 167 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdX as a double 168 | 169 | % --- Executes during object creation, after setting all properties. 170 | function Sigma_stdX_CreateFcn(hObject, eventdata, handles) 171 | % hObject handle to Sigma_stdX (see GCBO) 172 | % eventdata reserved - to be defined in a future version of MATLAB 173 | % handles empty - handles not created until after all CreateFcns called 174 | 175 | % Hint: edit controls usually have a white background on Windows. 176 | % See ISPC and COMPUTER. 177 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 178 | set(hObject, 'BackgroundColor', 'white'); 179 | end 180 | 181 | function Sigma_stdY_Callback(hObject, eventdata, handles) 182 | % hObject handle to Sigma_stdY (see GCBO) 183 | % eventdata reserved - to be defined in a future version of MATLAB 184 | % handles structure with handles and user data (see GUIDATA) 185 | 186 | % Hints: get(hObject, 'String') returns contents of Sigma_stdY as text 187 | % str2double(get(hObject, 'String')) returns contents of Sigma_stdY as a double 188 | 189 | % --- Executes during object creation, after setting all properties. 190 | function Sigma_stdY_CreateFcn(hObject, eventdata, handles) 191 | % hObject handle to Sigma_stdY (see GCBO) 192 | % eventdata reserved - to be defined in a future version of MATLAB 193 | % handles empty - handles not created until after all CreateFcns called 194 | 195 | % Hint: edit controls usually have a white background on Windows. 196 | % See ISPC and COMPUTER. 197 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 198 | set(hObject, 'BackgroundColor', 'white'); 199 | end 200 | 201 | function Sigma_std_Theta_Callback(hObject, eventdata, handles) 202 | % hObject handle to Sigma_std_Theta (see GCBO) 203 | % eventdata reserved - to be defined in a future version of MATLAB 204 | % handles structure with handles and user data (see GUIDATA) 205 | 206 | % Hints: get(hObject, 'String') returns contents of Sigma_std_Theta as text 207 | % str2double(get(hObject, 'String')) returns contents of Sigma_std_Theta as a double 208 | 209 | % --- Executes during object creation, after setting all properties. 210 | function Sigma_std_Theta_CreateFcn(hObject, eventdata, handles) 211 | % hObject handle to Sigma_std_Theta (see GCBO) 212 | % eventdata reserved - to be defined in a future version of MATLAB 213 | % handles empty - handles not created until after all CreateFcns called 214 | 215 | % Hint: edit controls usually have a white background on Windows. 216 | % See ISPC and COMPUTER. 217 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 218 | set(hObject, 'BackgroundColor', 'white'); 219 | end 220 | 221 | function Linear_velocity_Callback(hObject, eventdata, handles) 222 | % hObject handle to Linear_velocity (see GCBO) 223 | % eventdata reserved - to be defined in a future version of MATLAB 224 | % handles structure with handles and user data (see GUIDATA) 225 | 226 | % Hints: get(hObject, 'String') returns contents of Linear_velocity as text 227 | % str2double(get(hObject, 'String')) returns contents of Linear_velocity as a double 228 | 229 | % --- Executes during object creation, after setting all properties. 230 | function Linear_velocity_CreateFcn(hObject, eventdata, handles) 231 | % hObject handle to Linear_velocity (see GCBO) 232 | % eventdata reserved - to be defined in a future version of MATLAB 233 | % handles empty - handles not created until after all CreateFcns called 234 | 235 | % Hint: edit controls usually have a white background on Windows. 236 | % See ISPC and COMPUTER. 237 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 238 | set(hObject, 'BackgroundColor', 'white'); 239 | end 240 | 241 | function Frequency_Callback(hObject, eventdata, handles) 242 | % hObject handle to Frequency (see GCBO) 243 | % eventdata reserved - to be defined in a future version of MATLAB 244 | % handles structure with handles and user data (see GUIDATA) 245 | 246 | % Hints: get(hObject, 'String') returns contents of Frequency as text 247 | % str2double(get(hObject, 'String')) returns contents of Frequency as a double 248 | 249 | % --- Executes during object creation, after setting all properties. 250 | function Frequency_CreateFcn(hObject, eventdata, handles) 251 | % hObject handle to Frequency (see GCBO) 252 | % eventdata reserved - to be defined in a future version of MATLAB 253 | % handles empty - handles not created until after all CreateFcns called 254 | 255 | % Hint: edit controls usually have a white background on Windows. 256 | % See ISPC and COMPUTER. 257 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 258 | set(hObject, 'BackgroundColor', 'white'); 259 | end 260 | 261 | function landmarks_edit_Callback(hObject, eventdata, handles) 262 | % hObject handle to landmarks_edit (see GCBO) 263 | % eventdata reserved - to be defined in a future version of MATLAB 264 | % handles structure with handles and user data (see GUIDATA) 265 | 266 | % Hints: get(hObject, 'String') returns contents of landmarks_edit as text 267 | % str2double(get(hObject, 'String')) returns contents of landmarks_edit as a double 268 | 269 | % --- Executes during object creation, after setting all properties. 270 | function landmarks_edit_CreateFcn(hObject, eventdata, handles) 271 | % hObject handle to landmarks_edit (see GCBO) 272 | % eventdata reserved - to be defined in a future version of MATLAB 273 | % handles empty - handles not created until after all CreateFcns called 274 | 275 | % Hint: edit controls usually have a white background on Windows. 276 | % See ISPC and COMPUTER. 277 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 278 | set(hObject, 'BackgroundColor', 'white'); 279 | end 280 | 281 | % --- Executes on button press in Open_landmarks. 282 | function Open_landmarks_Callback(hObject, eventdata, handles) 283 | % hObject handle to Open_landmarks (see GCBO) 284 | % eventdata reserved - to be defined in a future version of MATLAB 285 | % handles structure with handles and user data (see GUIDATA) 286 | %[filename_land, pathname_land, filterindex_land] = uigetfile('*.mat', ... 287 | % 'Select file', 'MultiSelect', 'off'); 288 | %%landmarks = load(filename_land); 289 | %filename_land = mat2cell(filename_land, 1); 290 | %filename_vet = cell2mat(filename_land(1)); 291 | %lm_struct = uiimport([pathname_land filename_vet]); 292 | %landmarks = lm_struct.lm; 293 | %handles.landmarks = landmarks; 294 | %guidata(hObject, handles); 295 | [landmarks, filename] = OpenFile(); 296 | handles.landmarks = landmarks; 297 | guidata(hObject, handles); 298 | set(handles.landmarks_edit, 'String', filename); 299 | 300 | function waypoints_edit_Callback(hObject, eventdata, handles) 301 | % hObject handle to waypoints_edit (see GCBO) 302 | % eventdata reserved - to be defined in a future version of MATLAB 303 | % handles structure with handles and user data (see GUIDATA) 304 | 305 | % Hints: get(hObject, 'String') returns contents of waypoints_edit as text 306 | % str2double(get(hObject, 'String')) returns contents of waypoints_edit as a double 307 | 308 | % --- Executes during object creation, after setting all properties. 309 | function waypoints_edit_CreateFcn(hObject, eventdata, handles) 310 | % hObject handle to waypoints_edit (see GCBO) 311 | % eventdata reserved - to be defined in a future version of MATLAB 312 | % handles empty - handles not created until after all CreateFcns called 313 | 314 | % Hint: edit controls usually have a white background on Windows. 315 | % See ISPC and COMPUTER. 316 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 317 | set(hObject, 'BackgroundColor', 'white'); 318 | end 319 | 320 | % --- Executes on button press in Open_waypoints. 321 | function Open_waypoints_Callback(hObject, eventdata, handles) 322 | % hObject handle to Open_waypoints (see GCBO) 323 | % eventdata reserved - to be defined in a future version of MATLAB 324 | % handles structure with handles and user data (see GUIDATA) 325 | %[filename_waypoints, pathname_waypoints, filterindex_waypoints] = uigetfile('*.mat', ... 326 | % 'Select file', 'MultiSelect', 'off'); 327 | %filename_waypoints = mat2cell(filename_waypoints, 1); 328 | %filename_vet2 = cell2mat(filename_waypoints(1)); 329 | %waypoints_struct = uiimport([pathname_waypoints filename_vet2]); 330 | %waypoints_struct = load([pathname_waypoints filename_waypoints]); 331 | %waypoints = waypoints_struct.waypoints; 332 | %handles.waypoints = waypoints; 333 | %guidata(hObject, handles); 334 | [waypoints, filename] = OpenFile(); 335 | handles.waypoints = waypoints; 336 | guidata(hObject, handles); 337 | set(handles.waypoints_edit, 'String', filename); 338 | 339 | function [variable, filename] = OpenFile() 340 | [filename, pathname] = uigetfile('*.mat', ... 341 | 'Select file', 'MultiSelect', 'off'); 342 | if (isequal(filename, 0) || isequal(pathname, 0)) 343 | %Operazione da eseguire se si e' premuto Cancel 344 | variable = 0; 345 | filename = 'Default'; 346 | else 347 | structure = load([pathname, filename]); 348 | fieldname = fieldnames(structure); 349 | variable = getfield(structure, char(fieldname)); 350 | end 351 | 352 | function X0_X_Callback(hObject, eventdata, handles) 353 | % hObject handle to X0_X (see GCBO) 354 | % eventdata reserved - to be defined in a future version of MATLAB 355 | % handles structure with handles and user data (see GUIDATA) 356 | 357 | % Hints: get(hObject, 'String') returns contents of X0_X as text 358 | % str2double(get(hObject, 'String')) returns contents of X0_X as a double 359 | 360 | % --- Executes during object creation, after setting all properties. 361 | function X0_X_CreateFcn(hObject, eventdata, handles) 362 | % hObject handle to X0_X (see GCBO) 363 | % eventdata reserved - to be defined in a future version of MATLAB 364 | % handles empty - handles not created until after all CreateFcns called 365 | 366 | % Hint: edit controls usually have a white background on Windows. 367 | % See ISPC and COMPUTER. 368 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 369 | set(hObject, 'BackgroundColor', 'white'); 370 | end 371 | 372 | function X0_Y_Callback(hObject, eventdata, handles) 373 | % hObject handle to X0_Y (see GCBO) 374 | % eventdata reserved - to be defined in a future version of MATLAB 375 | % handles structure with handles and user data (see GUIDATA) 376 | 377 | % Hints: get(hObject, 'String') returns contents of X0_Y as text 378 | % str2double(get(hObject, 'String')) returns contents of X0_Y as a double 379 | 380 | % --- Executes during object creation, after setting all properties. 381 | function X0_Y_CreateFcn(hObject, eventdata, handles) 382 | % hObject handle to X0_Y (see GCBO) 383 | % eventdata reserved - to be defined in a future version of MATLAB 384 | % handles empty - handles not created until after all CreateFcns called 385 | 386 | % Hint: edit controls usually have a white background on Windows. 387 | % See ISPC and COMPUTER. 388 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 389 | set(hObject, 'BackgroundColor', 'white'); 390 | end 391 | 392 | function X0_Theta_Callback(hObject, eventdata, handles) 393 | % hObject handle to X0_Theta (see GCBO) 394 | % eventdata reserved - to be defined in a future version of MATLAB 395 | % handles structure with handles and user data (see GUIDATA) 396 | 397 | % Hints: get(hObject, 'String') returns contents of X0_Theta as text 398 | % str2double(get(hObject, 'String')) returns contents of X0_Theta as a double 399 | 400 | % --- Executes during object creation, after setting all properties. 401 | function X0_Theta_CreateFcn(hObject, eventdata, handles) 402 | % hObject handle to X0_Theta (see GCBO) 403 | % eventdata reserved - to be defined in a future version of MATLAB 404 | % handles empty - handles not created until after all CreateFcns called 405 | 406 | % Hint: edit controls usually have a white background on Windows. 407 | % See ISPC and COMPUTER. 408 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 409 | set(hObject, 'BackgroundColor', 'white'); 410 | end 411 | 412 | function Mu0_X_Callback(hObject, eventdata, handles) 413 | % hObject handle to Mu0_X (see GCBO) 414 | % eventdata reserved - to be defined in a future version of MATLAB 415 | % handles structure with handles and user data (see GUIDATA) 416 | 417 | % Hints: get(hObject, 'String') returns contents of Mu0_X as text 418 | % str2double(get(hObject, 'String')) returns contents of Mu0_X as a double 419 | 420 | % --- Executes during object creation, after setting all properties. 421 | function Mu0_X_CreateFcn(hObject, eventdata, handles) 422 | % hObject handle to Mu0_X (see GCBO) 423 | % eventdata reserved - to be defined in a future version of MATLAB 424 | % handles empty - handles not created until after all CreateFcns called 425 | 426 | % Hint: edit controls usually have a white background on Windows. 427 | % See ISPC and COMPUTER. 428 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 429 | set(hObject, 'BackgroundColor', 'white'); 430 | end 431 | 432 | function Mu0_Y_Callback(hObject, eventdata, handles) 433 | % hObject handle to Mu0_Y (see GCBO) 434 | % eventdata reserved - to be defined in a future version of MATLAB 435 | % handles structure with handles and user data (see GUIDATA) 436 | 437 | % Hints: get(hObject, 'String') returns contents of Mu0_Y as text 438 | % str2double(get(hObject, 'String')) returns contents of Mu0_Y as a double 439 | 440 | % --- Executes during object creation, after setting all properties. 441 | function Mu0_Y_CreateFcn(hObject, eventdata, handles) 442 | % hObject handle to Mu0_Y (see GCBO) 443 | % eventdata reserved - to be defined in a future version of MATLAB 444 | % handles empty - handles not created until after all CreateFcns called 445 | 446 | % Hint: edit controls usually have a white background on Windows. 447 | % See ISPC and COMPUTER. 448 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 449 | set(hObject, 'BackgroundColor', 'white'); 450 | end 451 | 452 | function Mu0_Theta_Callback(hObject, eventdata, handles) 453 | % hObject handle to Mu0_Theta (see GCBO) 454 | % eventdata reserved - to be defined in a future version of MATLAB 455 | % handles structure with handles and user data (see GUIDATA) 456 | 457 | % Hints: get(hObject, 'String') returns contents of Mu0_Theta as text 458 | % str2double(get(hObject, 'String')) returns contents of Mu0_Theta as a double 459 | 460 | % --- Executes during object creation, after setting all properties. 461 | function Mu0_Theta_CreateFcn(hObject, eventdata, handles) 462 | % hObject handle to Mu0_Theta (see GCBO) 463 | % eventdata reserved - to be defined in a future version of MATLAB 464 | % handles empty - handles not created until after all CreateFcns called 465 | 466 | % Hint: edit controls usually have a white background on Windows. 467 | % See ISPC and COMPUTER. 468 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 469 | set(hObject, 'BackgroundColor', 'white'); 470 | end 471 | 472 | function Sensor_range_Callback(hObject, eventdata, handles) 473 | % hObject handle to Sensor_range (see GCBO) 474 | % eventdata reserved - to be defined in a future version of MATLAB 475 | % handles structure with handles and user data (see GUIDATA) 476 | 477 | % Hints: get(hObject, 'String') returns contents of Sensor_range as text 478 | % str2double(get(hObject, 'String')) returns contents of Sensor_range as a double 479 | 480 | % --- Executes during object creation, after setting all properties. 481 | function Sensor_range_CreateFcn(hObject, eventdata, handles) 482 | % hObject handle to Sensor_range (see GCBO) 483 | % eventdata reserved - to be defined in a future version of MATLAB 484 | % handles empty - handles not created until after all CreateFcns called 485 | 486 | % Hint: edit controls usually have a white background on Windows. 487 | % See ISPC and COMPUTER. 488 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 489 | set(hObject, 'BackgroundColor', 'white'); 490 | end 491 | 492 | function Control_noise_r_Callback(hObject, eventdata, handles) 493 | % hObject handle to Control_noise_r (see GCBO) 494 | % eventdata reserved - to be defined in a future version of MATLAB 495 | % handles structure with handles and user data (see GUIDATA) 496 | 497 | % Hints: get(hObject, 'String') returns contents of Control_noise_r as text 498 | % str2double(get(hObject, 'String')) returns contents of Control_noise_r as a double 499 | 500 | % --- Executes during object creation, after setting all properties. 501 | function Control_noise_r_CreateFcn(hObject, eventdata, handles) 502 | % hObject handle to Control_noise_r (see GCBO) 503 | % eventdata reserved - to be defined in a future version of MATLAB 504 | % handles empty - handles not created until after all CreateFcns called 505 | 506 | % Hint: edit controls usually have a white background on Windows. 507 | % See ISPC and COMPUTER. 508 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 509 | set(hObject, 'BackgroundColor', 'white'); 510 | end 511 | 512 | function Control_noise_angle_Callback(hObject, eventdata, handles) 513 | % hObject handle to Control_noise_angle (see GCBO) 514 | % eventdata reserved - to be defined in a future version of MATLAB 515 | % handles structure with handles and user data (see GUIDATA) 516 | 517 | % Hints: get(hObject, 'String') returns contents of Control_noise_angle as text 518 | % str2double(get(hObject, 'String')) returns contents of Control_noise_angle as a double 519 | 520 | % --- Executes during object creation, after setting all properties. 521 | function Control_noise_angle_CreateFcn(hObject, eventdata, handles) 522 | % hObject handle to Control_noise_angle (see GCBO) 523 | % eventdata reserved - to be defined in a future version of MATLAB 524 | % handles empty - handles not created until after all CreateFcns called 525 | 526 | % Hint: edit controls usually have a white background on Windows. 527 | % See ISPC and COMPUTER. 528 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 529 | set(hObject, 'BackgroundColor', 'white'); 530 | end 531 | 532 | function Sensor_noise_r_Callback(hObject, eventdata, handles) 533 | % hObject handle to Sensor_noise_r (see GCBO) 534 | % eventdata reserved - to be defined in a future version of MATLAB 535 | % handles structure with handles and user data (see GUIDATA) 536 | 537 | % Hints: get(hObject, 'String') returns contents of Sensor_noise_r as text 538 | % str2double(get(hObject, 'String')) returns contents of Sensor_noise_r as a double 539 | 540 | % --- Executes during object creation, after setting all properties. 541 | function Sensor_noise_r_CreateFcn(hObject, eventdata, handles) 542 | % hObject handle to Sensor_noise_r (see GCBO) 543 | % eventdata reserved - to be defined in a future version of MATLAB 544 | % handles empty - handles not created until after all CreateFcns called 545 | 546 | % Hint: edit controls usually have a white background on Windows. 547 | % See ISPC and COMPUTER. 548 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 549 | set(hObject, 'BackgroundColor', 'white'); 550 | end 551 | 552 | function Sensor_noise_angle_Callback(hObject, eventdata, handles) 553 | % hObject handle to Sensor_noise_angle (see GCBO) 554 | % eventdata reserved - to be defined in a future version of MATLAB 555 | % handles structure with handles and user data (see GUIDATA) 556 | 557 | % Hints: get(hObject, 'String') returns contents of Sensor_noise_angle as text 558 | % str2double(get(hObject, 'String')) returns contents of Sensor_noise_angle as a double 559 | 560 | % --- Executes during object creation, after setting all properties. 561 | function Sensor_noise_angle_CreateFcn(hObject, eventdata, handles) 562 | % hObject handle to Sensor_noise_angle (see GCBO) 563 | % eventdata reserved - to be defined in a future version of MATLAB 564 | % handles empty - handles not created until after all CreateFcns called 565 | 566 | % Hint: edit controls usually have a white background on Windows. 567 | % See ISPC and COMPUTER. 568 | if ispc && isequal(get(hObject, 'BackgroundColor'), get(0, 'defaultUicontrolBackgroundColor')) 569 | set(hObject, 'BackgroundColor', 'white'); 570 | end 571 | 572 | % --- Executes on button press in Zoom. 573 | function Zoom_Callback(hObject, eventdata, handles) 574 | % hObject handle to Zoom (see GCBO) 575 | % eventdata reserved - to be defined in a future version of MATLAB 576 | % handles structure with handles and user data (see GUIDATA) 577 | zoom on; 578 | 579 | % --- Executes on button press in Pan. 580 | function Pan_Callback(hObject, eventdata, handles) 581 | % hObject handle to Pan (see GCBO) 582 | % eventdata reserved - to be defined in a future version of MATLAB 583 | % handles structure with handles and user data (see GUIDATA) 584 | pan on; 585 | 586 | function handles = initialize_gui(hObject, handles) 587 | % If the metricdata field is present and the reset flag is false, it means 588 | % we are we are just re-initializing a GUI by calling it from the cmd line 589 | % while it is up. So, bail out as we dont want to reset the data. 590 | set(handles.Lin_v, 'Visible', 'OFF'); 591 | set(handles.Linear_velocity, 'Visible', 'OFF'); 592 | cla(handles.Plot, 'reset'); 593 | %set(handles.Plot, 'Color', [0.3 0.3 0.3]); 594 | cla(handles.Legend, 'reset'); 595 | %set(handles.Legend, 'Color', [0.3 0.3 0.3]); 596 | set(handles.Legend, 'Visible', 'OFF'); 597 | set(handles.Legend, 'XTick', []); 598 | set(handles.Legend, 'YTick', []); 599 | set(handles.Default, 'Enable', 'ON'); 600 | set(handles.Default, 'Value', 1); 601 | set_Default(hObject, handles); 602 | disable_fields(hObject, handles); 603 | 604 | function set_Default(hObject, handles) 605 | %set(handles.Open_landmarks, 'Enable', 'OFF'); 606 | set(handles.landmarks_edit, 'String', 'landmarks.mat'); 607 | handles.landmarks = 0; 608 | guidata(hObject, handles); 609 | %set(handles.Open_waypoints, 'Enable', 'OFF'); 610 | set(handles.waypoints_edit, 'String', 'waypoints.mat'); 611 | handles.waypoints = 0; 612 | guidata(hObject, handles); 613 | set(handles.X0_X, 'String', 0); 614 | set(handles.X0_Y, 'String', 0); 615 | set(handles.X0_Theta, 'String', 0); 616 | set(handles.Mu0_X, 'String', 0); 617 | set(handles.Mu0_Y, 'String', 0); 618 | set(handles.Mu0_Theta, 'String', 0); 619 | %set(handles.Linear_velocity, 'String', 0.3); 620 | set(handles.Frequency, 'String', 10); 621 | set(handles.Control_noise_r, 'String', 0.03); 622 | set(handles.Control_noise_angle, 'String', 'pi/180'); 623 | set(handles.Sensor_noise_r, 'String', 0.05); 624 | set(handles.Sensor_noise_angle, 'String', 'pi/180'); 625 | set(handles.Sensor_range, 'String', 4); 626 | set(handles.Sigma_stdX, 'String', 0); 627 | set(handles.Sigma_stdY, 'String', 0); 628 | set(handles.Sigma_std_Theta, 'String', 0); 629 | set(handles.loops, 'String', 3); 630 | 631 | function disable_fields(hObject, handles) 632 | set(handles.Open_landmarks, 'Enable', 'OFF'); 633 | set(handles.landmarks_edit, 'Enable', 'OFF'); 634 | set(handles.Open_waypoints, 'Enable', 'OFF'); 635 | set(handles.waypoints_edit, 'Enable', 'OFF'); 636 | set(handles.X0_X, 'Enable', 'OFF'); 637 | set(handles.X0_Y, 'Enable', 'OFF'); 638 | set(handles.X0_Theta, 'Enable', 'OFF'); 639 | set(handles.Mu0_X, 'Enable', 'OFF'); 640 | set(handles.Mu0_Y, 'Enable', 'OFF'); 641 | set(handles.Mu0_Theta, 'Enable', 'OFF'); 642 | %set(handles.Linear_velocity, 'Enable', 'OFF'); 643 | set(handles.Frequency, 'Enable', 'OFF'); 644 | set(handles.Control_noise_r, 'Enable', 'OFF'); 645 | set(handles.Control_noise_angle, 'Enable', 'OFF'); 646 | set(handles.Sensor_noise_r, 'Enable', 'OFF'); 647 | set(handles.Sensor_noise_angle, 'Enable', 'OFF'); 648 | set(handles.Sensor_range, 'Enable', 'OFF'); 649 | set(handles.Sigma_stdX, 'Enable', 'OFF'); 650 | set(handles.Sigma_stdY, 'Enable', 'OFF'); 651 | set(handles.Sigma_std_Theta, 'Enable', 'OFF'); 652 | set(handles.loops, 'Enable', 'OFF'); 653 | 654 | function enable_fields(hObject, handles) 655 | set(handles.Open_landmarks, 'Enable', 'ON'); 656 | set(handles.landmarks_edit, 'Enable', 'ON'); 657 | set(handles.Open_waypoints, 'Enable', 'ON'); 658 | set(handles.waypoints_edit, 'Enable', 'ON'); 659 | set(handles.X0_X, 'Enable', 'ON'); 660 | set(handles.X0_Y, 'Enable', 'ON'); 661 | set(handles.X0_Theta, 'Enable', 'ON'); 662 | set(handles.Mu0_X, 'Enable', 'ON'); 663 | set(handles.Mu0_Y, 'Enable', 'ON'); 664 | set(handles.Mu0_Theta, 'Enable', 'ON'); 665 | %set(handles.Linear_velocity, 'Enable', 'ON'); 666 | set(handles.Frequency, 'Enable', 'ON'); 667 | set(handles.Control_noise_r, 'Enable', 'ON'); 668 | set(handles.Control_noise_angle, 'Enable', 'ON'); 669 | set(handles.Sensor_noise_r, 'Enable', 'ON'); 670 | set(handles.Sensor_noise_angle, 'Enable', 'ON'); 671 | set(handles.Sensor_range, 'Enable', 'ON'); 672 | set(handles.Sigma_stdX, 'Enable', 'ON'); 673 | set(handles.Sigma_stdY, 'Enable', 'ON'); 674 | set(handles.Sigma_std_Theta, 'Enable', 'ON'); 675 | set(handles.loops, 'Enable', 'ON'); 676 | 677 | function set_Legend(hObject, handles) 678 | im = imread('legend5.png'); 679 | set(handles.Legend, 'HandleVisibility', 'ON'); 680 | axes(handles.Legend); 681 | image(im); 682 | set(handles.Legend, 'XTick', []); 683 | set(handles.Legend, 'YTick', []); 684 | 685 | function [X0, Mu0, vl, freq, R, Q, lm_vis, Sigma0, LOOPS, landmarks, waypoints] = create_var(hObject, handles) 686 | X0_X = eval(get(handles.X0_X, 'String')); 687 | X0_Y = eval(get(handles.X0_Y, 'String')); 688 | X0_Theta = eval(get(handles.X0_Theta, 'String')); 689 | Mu0_X = eval(get(handles.Mu0_X, 'String')); 690 | Mu0_Y = eval(get(handles.Mu0_Y, 'String')); 691 | Mu0_Theta = eval(get(handles.Mu0_Theta, 'String')); 692 | Linear_velocity = eval(get(handles.Linear_velocity, 'String')); 693 | Frequency = eval(get(handles.Frequency, 'String')); 694 | Control_noise_r = eval(get(handles.Control_noise_r, 'String')); 695 | Control_noise_angle = eval(get(handles.Control_noise_angle, 'String')); 696 | Sensor_noise_r = eval(get(handles.Sensor_noise_r, 'String')); 697 | Sensor_noise_angle = eval(get(handles.Sensor_noise_angle, 'String')); 698 | Sensor_range = eval(get(handles.Sensor_range, 'String')); 699 | Sigma_stdX = eval(get(handles.Sigma_stdX, 'String')); 700 | Sigma_stdY = eval(get(handles.Sigma_stdY, 'String')); 701 | Sigma_std = eval(get(handles.Sigma_std_Theta, 'String')); 702 | loops = eval(get(handles.loops, 'String')); 703 | 704 | X0 = [X0_X; X0_Y; X0_Theta]; 705 | Mu0 = [Mu0_X; Mu0_Y; Mu0_Theta]; 706 | vl = Linear_velocity; 707 | freq = Frequency; 708 | if (Sensor_noise_r <= 1e-3) 709 | Sensor_noise_r = 1e-3; 710 | end 711 | if (Sensor_noise_angle <= 1e-3) 712 | Sensor_noise_angle = 1e-3; 713 | end 714 | R = [Control_noise_r, 0; 0, Control_noise_angle].^2; 715 | Q = [Sensor_noise_r, 0; 0, Sensor_noise_angle].^2; 716 | lm_vis = Sensor_range; 717 | Sigma0 = [Sigma_stdX, 0, 0; 0, Sigma_stdY, 0; 0, 0, Sigma_std]; 718 | LOOPS = loops; 719 | 720 | if isfield(handles, 'landmarks') 721 | landmarks = handles.landmarks; 722 | else 723 | landmarks = 0; 724 | end 725 | if isfield(handles, 'waypoints') 726 | waypoints = handles.waypoints; 727 | else 728 | waypoints = 0; 729 | end 730 | 731 | function Esercitazione5(X0, Mu0, vl, freq, R, Q, lm_vis, Sigma0, LOOPS, landmarks, waypoints, hObject, handles) 732 | %Funzione per il calcolo della mappa e della localizzazione del robot 733 | %Parametri IN: 734 | %landmarks : vettore contenente le coordinate dei landmarks 735 | %waypoints : vettore contenente le coordinate dei waypoints costituenti il percorso 736 | % seguito dal robot 737 | %lm_vis : parametro indicante il range sensoriale del robot 738 | 739 | global flag_stop; 740 | flag_stop = 0; 741 | axes(handles.Plot); 742 | 743 | % Inizializzazione Landmark e Boe 744 | if (~landmarks) 745 | landmarks = [-1:11, 1:2:9, 11*ones(1, 12), 9*ones(1, 4), -1:10, 1:2:7; 746 | -ones(1, 13), ones(1, 5), 0:11, 3:2:9, 11*ones(1, 12), 9*ones(1, 4)]; 747 | end 748 | if (~waypoints) 749 | waypoints = [0 10 10 0 0; 750 | 0 0 10 10 0]; 751 | end 752 | if (get(handles.Default, 'Value')) 753 | lm_vis = 4; 754 | end 755 | 756 | if (size(landmarks, 1) == 2) 757 | landmarks(3, :) = 1:size(landmarks, 2); 758 | end 759 | 760 | % Impostazione della finestra di visualizzazione 761 | MAXX = max([max(landmarks(1, :)) max(waypoints(1, :))]); 762 | MAXY = max([max(landmarks(2, :)) max(waypoints(2, :))]); 763 | MINX = min([min(landmarks(1, :)) min(waypoints(1, :))]); 764 | MINY = min([min(landmarks(2, :)) min(waypoints(2, :))]); 765 | lim_axis = [MINX-1 MAXX+1 MINY-1 MAXY+1]; 766 | axis(lim_axis); 767 | x_step = (lim_axis(2)-lim_axis(1))/5; 768 | y_step = (lim_axis(4)-lim_axis(3))/5; 769 | set(handles.Plot, 'XTick', round(100*(lim_axis(1):x_step:lim_axis(2)))/100); 770 | set(handles.Plot, 'YTick', round(100*(lim_axis(3):y_step:lim_axis(4)))/100); 771 | 772 | map_factor = min(MAXX-MINX, MAXY-MINY); 773 | vl_factor = 30; 774 | %vl = map_factor / vl_factor; 775 | vl = 0.3; 776 | 777 | % Plot dei landmark e dei waypoints 778 | hold on; 779 | plot(landmarks(1, :), landmarks(2, :), 'ob'); 780 | plot(waypoints(1, :), waypoints(2, :), 'b.--'); 781 | 782 | % Inizializzazione dei parametri dello SLAM 783 | dt_period = 0; 784 | dt = 1/freq; 785 | theta = 0; 786 | 787 | % Inizializzazione dei parametri di visualizzazione 788 | sense_step = 2; 789 | draw_step = 10; 790 | draw_every = 2; 791 | 792 | % Costante booleana per il tracciamento del percorso del robot. 793 | % True -> linea | False -> punti 794 | DRAW_PATH = true; 795 | 796 | current_b = 0; 797 | at_boa_tol = 0.03; 798 | % max_dir = 120*pi/180; 799 | max_de_dir = 120*pi/180; 800 | 801 | Xk = X0; 802 | odom = X0; 803 | Muk = Mu0; 804 | Sigma_k = Sigma0; 805 | 806 | dir = theta; 807 | 808 | dim_robot = 0.1; 809 | 810 | seen_lm = zeros(1, size(landmarks, 2)); 811 | 812 | % Inizializzazione del plot del robot, odometria, stime dei landmark, 813 | % covarianze del veicolo e covarianze dei landmark 814 | h2 = plot(0, 0, 'ro'); 815 | % h3 = plot(0, 0, 'go'); 816 | h4 = plot(0, 0, '+r'); 817 | vcov = plot(0, 0, 'r'); 818 | fcov = plot(0, 0, 'r'); 819 | 820 | cov_gain = 1; 821 | loop = 1; 822 | i_draw = 1; 823 | skip_count = 0; 824 | accPath = []; 825 | 826 | % colors = [1:-0.2:0; 0:0.2:1]'; 827 | % colors(:, 3) = 0; 828 | colors = [1 0 0; % r 829 | 0 1 0; % g 830 | 0 0 1; % b 831 | 0 1 1; % c 832 | 1 0 1; % m 833 | 1 0.5 0]; % o 834 | 835 | while (loop <= LOOPS) 836 | i_draw = i_draw + 1; 837 | 838 | h2path = plot(0, 0, 'r'); % Plot del percorso del robot 839 | % h2path = plot(0, 0, '-', 'color', colors(1+mod(loop-1, size(colors, 1)), :)); % Plot del percorso del robot 840 | 841 | % Plot della stima del robot, percorso stimato del robot, odometria, 842 | % stima della posizione dei landmark, covarianza del robot e dei 843 | % landmark 844 | if ((DRAW_PATH) && (~skip_count)), accPath = [accPath, Muk(1:2)]; end; 845 | skip_count = mod(skip_count+1, draw_every); 846 | 847 | if (~mod(i_draw, draw_step)) 848 | if (DRAW_PATH) 849 | set(h2path, 'XData', accPath(1, :), 'YData', accPath(2, :)); 850 | else 851 | set(h2path, 'XData', Muk(1), 'YData', Muk(2)); 852 | end 853 | 854 | set(h2, 'XData', Muk(1), 'YData', Muk(2)); 855 | % set(h3, 'XData', odom(1), 'YData', odom(2)); 856 | set(h4, 'XData', Muk(4:2:length(Muk)-1), 'YData', Muk(5:2:length(Muk))); 857 | if (Sigma_k(1:2, 1:2) ~= zeros(2)) 858 | pvcov = PlotEllipse(Muk(1:2), Sigma_k(1:2, 1:2), cov_gain); 859 | set(vcov, 'xdata', pvcov(1, :), 'ydata', pvcov(2, :)); 860 | end 861 | if (length(Muk) > 3) 862 | pfcov = PlotEllipseLm(Muk, Sigma_k, cov_gain); 863 | set(fcov, 'xdata', pfcov(1, :), 'ydata', pfcov(2, :)); 864 | end 865 | 866 | drawnow; 867 | 868 | if (flag_stop) 869 | return 870 | end 871 | 872 | if (DRAW_PATH), accPath = accPath(:, end); end; 873 | end 874 | 875 | % Calcolo della direzione verso la prossima boa 876 | next_b = waypoints(:, current_b + 1); 877 | dist_boa = sqrt((Xk(1:2)-next_b)' * (Xk(1:2)-next_b)); 878 | if (dist_boa < at_boa_tol) 879 | if (current_b + 1 == size(waypoints, 2)) 880 | current_b = 0; 881 | loop = loop + 1; 882 | else 883 | current_b = current_b + 1; 884 | end 885 | end 886 | de_dir = fixAngle(atan2(next_b(2)-Xk(2), next_b(1)-Xk(1)) - Xk(3) - dir); 887 | max_De = max_de_dir*dt; 888 | if (abs(de_dir) > max_De) 889 | de_dir = sign(de_dir)*max_De; 890 | end 891 | dir = dir + de_dir; 892 | 893 | % Applicazione del modello del moto al robot 894 | Xk(1:length(X0)) = motionModel(Xk(1:length(X0)), vl, dir, dim_robot, dt); 895 | 896 | % Aggiunta di rumore alla velocita' lineare e all'angolo di sterzata 897 | % del robot 898 | vl_n = vl + randn(1)*sqrt(R(1, 1)); 899 | dir_n = dir + randn(1)*sqrt(R(2, 2)); 900 | 901 | % Fase di predizione 902 | deX = vl_n*dt * sin(dir_n+Muk(3)); 903 | deY = vl_n*dt * cos(dir_n+Muk(3)); 904 | Gt = [1 0 -deX; 0 1 deY; 0 0 1]; 905 | Gu = [dt*cos(Muk(3)+dir_n) -deX; 906 | dt*sin(Muk(3)+dir_n) deY; 907 | dt*sin(dir_n)/dim_robot vl_n*dt*cos(dir_n)/dim_robot]; 908 | 909 | % Aggiornamento della matrice di covarianza 910 | Sigma_k(1:length(X0), 1:length(X0)) = Gt * Sigma_k(1:length(X0), 1:length(X0)) * Gt' + Gu*R*Gu'; 911 | if (length(Sigma_k) > 3) 912 | Sigma_k(1:length(X0), length(X0)+1:end) = Gt * Sigma_k(1:length(X0), length(X0)+1:end); 913 | Sigma_k(length(X0)+1:end, 1:length(X0)) = Sigma_k(1:length(X0), length(X0)+1:end)'; 914 | end 915 | 916 | % Applicazione del modello del moto alla stima del robot 917 | Muk(1:length(X0)) = motionModel(Muk, vl_n, dir_n, dim_robot, dt); 918 | 919 | % Applicazione del modello del moto all'odometria 920 | % odom = motionModel(odom, vl_n, dir_n, dim_robot, dt); 921 | 922 | dt_period = dt_period + dt; 923 | if (dt_period >= (sense_step*dt)) 924 | dt_period = 0; 925 | 926 | % Calcolo delle distanze tra il robot e i landmark 927 | dist_lm = sqrt(sum((landmarks(1:2, :) - repmat(Xk(1:2, :), 1, size(landmarks, 2))).^2)); 928 | id_vis = find(dist_lm < lm_vis); 929 | landmark_range = (landmarks(:, id_vis)); 930 | dx = landmark_range(1, :) - Xk(1); 931 | dy = landmark_range(2, :) - Xk(2); 932 | z_lm = [dist_lm(id_vis); fixAngle(atan2(dy, dx)-Xk(3)); landmark_range(3, :)]; 933 | z_lm_n = z_lm + randn(size(z_lm)).*repmat([sqrt(diag(Q)); 0], 1, size(z_lm, 2)); 934 | 935 | % Associazione dei landmark compresi all'interno del range di 936 | % visibilita' 937 | [z_old, z_new, seen_lm, idf] = landmark_associate(Mu0, Muk, z_lm_n, seen_lm); 938 | 939 | % Fase di aggiornamento 940 | len_z = size(z_old, 2); 941 | len_x = length(Muk); 942 | H = zeros(2*len_z, len_x); 943 | v = zeros(2*len_z, 1); 944 | QQ = zeros(2*len_z); 945 | 946 | for i = 1:len_z 947 | ii = 2*i + (-1:0); 948 | [z_h, H(ii, :)] = sensorModel(Mu0, Muk, idf(i)); 949 | v(ii) = [z_old(1, i)-z_h(1); 950 | fixAngle(z_old(2, i)-z_h(2))]; 951 | QQ(ii, ii) = Q; 952 | end 953 | 954 | % Calcolo del guadagno di Kalman 955 | S = symmetric_m(H * (Sigma_k * H') + QQ); 956 | %K = (Sigma_k * H') * (inv(S)); 957 | K = (Sigma_k * H') / S; 958 | Muk = Muk + K * v; 959 | I = eye(size(Sigma_k, 2)); 960 | Sigma_k = (I - K * H) * Sigma_k; 961 | Sigma_k = symmetric_m(Sigma_k); 962 | 963 | % Incremento delle dimensioni matriciali di Muk e Sigma_k 964 | for i = 1 : size(z_new, 2) 965 | r = z_new(1, i); 966 | b = z_new(2, i); 967 | len = length(Muk); 968 | 969 | Muk = [Muk; 970 | Muk(1) + r*cos(b+Muk(3)); 971 | Muk(2) + r*sin(b+Muk(3))]; 972 | 973 | deX = r * sin(b+Muk(3)); 974 | deY = r * cos(b+Muk(3)); 975 | Gv = [1 0 -deX; 0 1 deY]; 976 | Gz = [cos(Muk(3)+b) -deX; 977 | sin(Muk(3)+b) deY]; 978 | 979 | rng = len+1:len+2; 980 | Sigma_k(rng, rng) = Gv*Sigma_k(1:3, 1:3)*Gv' + Gz*Q*Gz'; 981 | Sigma_k(rng, 1:3) = Gv*Sigma_k(1:3, 1:3); 982 | Sigma_k(1:3, rng) = Sigma_k(rng, 1:3)'; 983 | if (len > 3) 984 | rnm = 4:len; 985 | Sigma_k(rng, rnm) = Gv*Sigma_k(1:3, rnm); 986 | Sigma_k(rnm, rng) = Sigma_k(rng, rnm)'; 987 | end 988 | 989 | end 990 | end 991 | end 992 | 993 | function S = symmetric_m(S) 994 | % Funzione per mantenere la forma simmetrica di una matrice ed ovviare ad 995 | % eventuali errori di arrotondamento scaturiti dai calcoli 996 | S = (S+S')*0.5; 997 | 998 | function el = PlotEllipse(x, P, nSigma) 999 | % Funzione per il tracciamento di una singola ellisse di covarianza 1000 | N = 8; 1001 | step = 2*pi/N; 1002 | if (all(diag(P))) 1003 | [V, D] = eig(P); 1004 | y = nSigma * [cos(0:step:2*pi); sin(0:step:2*pi)]; 1005 | el = V*sqrtm(D)*y; 1006 | el = [el el(:, 1)] + repmat(x, 1, size(el, 2)+1); 1007 | el(:, end) = NaN; 1008 | end 1009 | 1010 | function el = PlotEllipseLm(x, P, nSigma) 1011 | % Funzione per il tracciamento di una serie di ellissi di covarianza 1012 | el = []; 1013 | for lm_id = 1:(length(x) - 3)/2 1014 | i = 2*lm_id - 1; 1015 | xx = x(i+3:i+4); 1016 | PP = P(i+3:i+4, i+3:i+4); 1017 | el = [el PlotEllipse(xx, PP, nSigma)]; 1018 | end 1019 | 1020 | function angle = fixAngle(angle) 1021 | %Funzione che corregge l'angolo riportandolo nell'intervallo (-180, +180) 1022 | %Parametri IN: 1023 | %angle : angolo da correggere 1024 | %Parametri OUT: 1025 | %fixed_angle : angolo corretto 1026 | 1027 | if angle > pi 1028 | angle = angle - 2*pi; 1029 | elseif angle < -pi 1030 | angle = angle + 2*pi; 1031 | end 1032 | 1033 | function [z_old, z_new, seen_lm, idf] = landmark_associate(Mu0, Muk, z_lm_n, seen_lm) 1034 | %Funzione che associa ai landmark gia' visti i nuovi landmark presenti nel 1035 | %range di visibilita' del robot 1036 | %Parametri IN: 1037 | %Mu0 : stima della posizione iniziale 1038 | %Muk : vettore delle stime della posizione del robot e dei landmark visti 1039 | %z_lm_n : vettore delle distanze tra il robot e i landmark visti 1040 | %seen_lm : vettore che contiene i flag rappresentanti l'ordine dei landmark 1041 | %visti 1042 | %Parametri OUT: 1043 | %z_old : vettore che contiene le distanze dei landmark gia' associati 1044 | %z_new : vettore che contiene le distanze dei landmark appena associati 1045 | %seen_lm : vettore aggiornato che contiene i flag rappresentanti l'ordine 1046 | %dei landmark visti 1047 | %idf : indice aggiornato dei landmark gia' associati 1048 | 1049 | %Inizializzazione dei vettori delle misure e degli indici dei landmark 1050 | z_old = []; z_new = []; 1051 | idf = []; idn = []; 1052 | 1053 | for i = 1:size(z_lm_n, 2) 1054 | j = z_lm_n(3, i); 1055 | if (~seen_lm(j)) %nuovo landmark da associare 1056 | z_new = [z_new z_lm_n(:, i)]; 1057 | idn = [idn j]; 1058 | else %landmark gia' associato 1059 | z_old = [z_old z_lm_n(:, i)]; 1060 | idf = [idf seen_lm(j)]; 1061 | end 1062 | end 1063 | 1064 | %contrassegna i landmark visti con il rispettivo ordine di avvistamento 1065 | Nlm = (length(Muk) - length(Mu0)) / 2; 1066 | seen_lm(idn) = Nlm + (1:size(z_new, 2)); 1067 | 1068 | function update = motionModel(current, vl, dir, dim_robot, dt) 1069 | %Funzione che applica il modello del moto al robot 1070 | %Parametri IN: 1071 | %current : posizione iniziale del robot 1072 | %vl : velocita' lineare 1073 | %dir : angolo di direzione del movimento del robot 1074 | %dim_robot : dimensione fisica del robot 1075 | %dt : periodo di campionamento 1076 | %Parametri OUT: 1077 | %update : posizione del robot aggiornata dopo l'applicazione del modello 1078 | %del moto 1079 | 1080 | update = [current(1) + dt*vl*cos(current(3)+dir); 1081 | current(2) + dt*vl*sin(current(3)+dir); 1082 | fixAngle(current(3) + dt*vl*sin(dir)/dim_robot)]; 1083 | 1084 | function [z_h, H] = sensorModel(Mu0, Muk, lm_id) 1085 | %Funzione che applica il modello delle misure tra il robot e i landmark 1086 | %Parametri IN: 1087 | %Mu0 : stima della posizione iniziale 1088 | %Muk : vettore delle stime della posizione del robot e dei landmark visti 1089 | %lm_id : vettore degli indici relativi ai landmark associati 1090 | %Parametri OUT: 1091 | %z_h : vettore che contiene la distanza predetta in coordinate polari 1092 | %H : Jacobiano 1093 | 1094 | %Inizializzazione dei parametri 1095 | dim = length(Mu0); 1096 | pos = dim + lm_id*2 - 1; 1097 | H = zeros(2, length(Muk)); 1098 | 1099 | muJ = Muk(pos:pos+1); 1100 | delta = [muJ(1)-Muk(1); muJ(2)-Muk(2)]; 1101 | 1102 | q = sqrt(delta.'*delta); 1103 | angle = fixAngle(atan2(delta(2), delta(1)) - Muk(3)); 1104 | 1105 | z_h = [q; angle]; 1106 | 1107 | H(:, 1:dim) = [-(delta(1)/q), -(delta(2)/q), 0; 1108 | delta(2)/(q*q), -(delta(1)/(q*q)), -1]; 1109 | H(:, pos:pos+1) = [ delta(1)/q, delta(2)/q; 1110 | -(delta(2)/(q*q)), delta(1)/(q*q)]; 1111 | --------------------------------------------------------------------------------