![]() |
EKF_PredictionPURPOSE
EKF Prediction step.
SYNOPSIS
function EKF=EKF_Prediction(EKF,d,Q)
DESCRIPTION
EKF Prediction step. Predicton step in a 'Extended Kalman Filter'. This is used when extending the trajectory (i.e., the state), with a new pose. It updates the mean and the covariance matrix. Parameters EKF: The filter to update d: The sensor reading providing the position of the new robot's pose from its predecesor pose. Q: The noise of the sensor providing 'd'. Outputs: EKF: The updated filter with the new pose. See also EKF_Update, TRO_Prediction, TRO_Update. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function EKF=EKF_Prediction(EKF,d,Q) 0002 % EKF Prediction step. 0003 % 0004 % Predicton step in a 'Extended Kalman Filter'. 0005 % This is used when extending the trajectory (i.e., the state), with a 0006 % new pose. 0007 % It updates the mean and the covariance matrix. 0008 % 0009 % Parameters 0010 % EKF: The filter to update 0011 % d: The sensor reading providing the position of the new robot's pose 0012 % from its predecesor pose. 0013 % Q: The noise of the sensor providing 'd'. 0014 % Outputs: 0015 % EKF: The updated filter with the new pose. 0016 % 0017 % See also EKF_Update, TRO_Prediction, TRO_Update. 0018 0019 % EKF prediction a la full SLAM, where the state is extended to estimate the 0020 % full trajectory. 0021 n=size(EKF.mu,1); 0022 0023 k=get(EKF.P_Filter,'dim'); % dimensionality of the pose space 0024 last=n-k+1:n; 0025 0026 P1=Pose(EKF.mu(last)); % mean for the previous pose 0027 0028 EKF.mu=[EKF.mu ; double(Relative2Absolute(P1,d))]; 0029 0030 % Jacobians used to linarly propagate the error from previous poses 0031 % to the new one 0032 [Jp Jd]=Relative2AbsoluteJacobian(P1,d); 0033 0034 Sp=EKF.Sigma(last,last); % Covariance of the previous pose 0035 Cp=EKF.Sigma(1:n,last); % Previous cross-covariance (last k columns) 0036 C=Cp*Jp'; 0037 0038 EKF.Sigma=[EKF.Sigma C ; C' Jp*Sp*Jp'+Jd*Q*Jd']; 0039 0040 |