![]() |
UpdatePURPOSE
Adds a new pose to an EKF filter.
SYNOPSIS
function EKF=Update(EKF,step,SR,Sn,overwritePose)
DESCRIPTION
Adds a new pose to an EKF filter. Updates an 'Extended Kalman Filter' adding a new pose to the estimated trajectory. It updates the mean and the covariance matrix. Parameters: EKF: The filter. step: Time slice. SR: The sensor readings. Sn: The noise of the sensors. overwritePose: Not used in the 'Extended Kalman Filter'. Returns: EKF: The updated 'Extended Kalman Filter'. The first sensor reading is used to extend the state (i.e., the trajectory) using EKF_Prediction, and the remaining ones are used to improve the estimation of the current robot's pose using EKF_Update. See also EKF_Prediction, EKF_Update CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function EKF=Update(EKF,step,SR,Sn,overwritePose) 0002 % Adds a new pose to an EKF filter. 0003 % 0004 % Updates an 'Extended Kalman Filter' adding a new pose to the estimated 0005 % trajectory. It updates the mean and the covariance matrix. 0006 % 0007 % Parameters: 0008 % EKF: The filter. 0009 % step: Time slice. 0010 % SR: The sensor readings. 0011 % Sn: The noise of the sensors. 0012 % overwritePose: Not used in the 'Extended Kalman Filter'. 0013 % Returns: 0014 % EKF: The updated 'Extended Kalman Filter'. 0015 % 0016 % The first sensor reading is used to extend the state (i.e., the 0017 % trajectory) using EKF_Prediction, and the remaining ones are used to 0018 % improve the estimation of the current robot's pose using EKF_Update. 0019 % 0020 % See also EKF_Prediction, EKF_Update 0021 0022 % overwritePose is not implemented in EkF. We keep the parameter 0023 % for compatibility with P_TRO filter . 0024 0025 ns=size(SR,2); % number of sensor readings 0026 0027 % First valid sensor reading is used for prediction (i.e., to extend the 0028 % state), and the rest of valid sensor readings for update. 0029 first=true; 0030 for i=1:ns 0031 d=SR{i}; 0032 if ~NullPose(d) 0033 if first 0034 EKF=EKF_Prediction(EKF,d,Sn{i}); 0035 0036 EKF.P_Filter=Update(EKF.P_Filter,step,false); 0037 0038 nStates=get(EKF.P_Filter,'nStates'); 0039 first=false; 0040 else 0041 EKF=EKF_Update(EKF,nStates-1,d,Sn{i}); 0042 end 0043 end 0044 end 0045 if first 0046 error('EKF step without any valid sensor reading'); 0047 end |