![]() |
TRO_PredictionPURPOSE
Prediction Step.
SYNOPSIS
function FF=TRO_Prediction(FF,d,Q,overwritePose)
DESCRIPTION
Prediction Step. This is used when extending the trajectory (i.e., the state), with a new pose. It updates the information matrix, the information vector, the mean (necessary as a linearization point for all filter operations) and updates F, the accumulation of linearizations of the displacement function (see Relative2AbsoluteJacobian) that is latter used to compute cross covariances of poses w.r.t. the new robot's pose. Parameters FF: 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'. overwritePose: If true, the last pose in the filter is removed and replaced by the new one. In this way the number of estimated elements actually does not grow and we end up with less states than simulation steps. Outputs: FF: The updated filter with the new pose. See also EKF_Prediction, EKF_Update, Update, Relative2AbsoluteJacobian. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function FF=TRO_Prediction(FF,d,Q,overwritePose) 0002 % Prediction Step. 0003 % 0004 % This is used when extending the trajectory (i.e., the state), with a 0005 % new pose. 0006 % It updates the information matrix, the information vector, the mean 0007 % (necessary as a linearization point for all filter operations) and 0008 % updates F, the accumulation of linearizations of the displacement 0009 % function (see Relative2AbsoluteJacobian) that is latter used to compute 0010 % cross covariances of poses w.r.t. the new robot's pose. 0011 % 0012 % Parameters 0013 % FF: The filter to update 0014 % d: The sensor reading providing the position of the new robot's pose 0015 % from its predecesor pose. 0016 % Q: The noise of the sensor providing 'd'. 0017 % overwritePose: If true, the last pose in the filter is removed and 0018 % replaced by the new one. In this way the number of 0019 % estimated elements actually does not grow and we end 0020 % up with less states than simulation steps. 0021 % Outputs: 0022 % FF: The updated filter with the new pose. 0023 % 0024 % See also EKF_Prediction, EKF_Update, Update, Relative2AbsoluteJacobian. 0025 0026 ne=size(FF); 0027 dim=get(FF.P_Filter,'dim'); % dimensionality of the pose space 0028 nStates=get(FF.P_Filter,'nStates'); 0029 step=get(FF.P_Filter,'nSteps'); 0030 0031 if overwritePose 0032 lastState=nStates-1; % Last valid state in the filter (in this case last 0033 % state minus one) 0034 newState=nStates; % Index where to store the new state (in this case) 0035 % we re-use last state) 0036 0037 last=(lastState-1)*dim+1:lastState*dim; 0038 new=(newState-1)*dim+1:newState*dim; 0039 0040 % First we undo the effect of the last _Prediction 0041 iQw12=FF.Lambda(new,new); 0042 Qw12=iQw12\eye(dim); 0043 Jp12=-Qw12*FF.Lambda(new,last); 0044 0045 FF.Lambda(last,last)=sparse2(FF.Lambda(last,last)-sparse2(Jp12'*iQw12*Jp12)); 0046 0047 etaN=FF.eta(new); 0048 FF.eta(last)=FF.eta(last)+Jp12'*etaN; 0049 0050 FF.F=FF.F*(Jp12\eye(dim))'; 0051 0052 % Now we define the data necessary to execute the prediction 0053 % below 0054 % Qw: Noise in global coordinates 0055 % P1d: last pose in the trajectory that we keep (as doubles). 0056 % Jp: Jacobian of Relative2Absolute w.r.t. the last pose in the 0057 % trajectory that we keep. 0058 % newPose: New robot position (mean). 0059 P1d=FF.mu(last); % Last know pose that we will keep 0060 P1=Pose(P1d); 0061 0062 P2=Pose(FF.mu(new));% Pose that will be removed 0063 0064 P3=Relative2Absolute(P2,d); % Pose to connect directly with P1 0065 newPos=double(P3); 0066 0067 [Jp23 Jd23]=Relative2AbsoluteJacobian(P2,d); 0068 0069 Jp=Jp23*Jp12; 0070 0071 Qw=Jp23*Qw12*Jp23'+Jd23*Q*Jd23'; 0072 else 0073 lastState=nStates; % Last valid state in the filter 0074 newState=nStates+1; % Index where to store the new state 0075 0076 last=(lastState-1)*dim+1:lastState*dim; 0077 new=(newState-1)*dim+1:newState*dim; 0078 0079 P1d=FF.mu(last); 0080 P1=Pose(P1d); % mean for the previous pose (used as a linealization 0081 % point 0082 0083 % Jacobians used to linarly propagate the error from previous poses 0084 % to the new one 0085 [Jp Jd]=Relative2AbsoluteJacobian(P1,d); 0086 0087 Qw=Jd*Q*Jd'; % displacement noise in global coordinates 0088 0089 newPos=double(Relative2Absolute(P1,d)); 0090 0091 % make room for the new data 0092 Zk=zeros(dim,1); 0093 FF.eta=[FF.eta ; Zk]; 0094 FF.mu=[FF.mu; Zk]; 0095 0096 Znk=sparse2(ne,dim); 0097 Zkk=sparse2(dim,dim); 0098 FF.Lambda=sparse2([FF.Lambda Znk ; Znk' Zkk ]); 0099 end 0100 0101 v=newPos-Jp*P1d; 0102 0103 iQw=Qw\eye(dim); 0104 etaN=iQw*v; 0105 0106 FF.eta(last)=FF.eta(last)-Jp'*etaN; 0107 FF.eta(new)=etaN; 0108 0109 FF.Lambda(last,last)=sparse2(FF.Lambda(last,last)+sparse2(Jp'*iQw*Jp)); 0110 0111 FF.Lambda(last,new)=sparse2(-Jp'*iQw); 0112 FF.Lambda(new,last)=sparse2(-iQw*Jp); 0113 FF.Lambda(new,new)=sparse2(iQw); 0114 0115 FF.mu(new)=newPos; 0116 0117 Sp=get(FF.PoseData{lastState},'covariance'); % Estimation for the previous pose 0118 FF.PoseData{lastState}=set(FF.PoseData{lastState},'crossCovarianceData',Sp*(FF.F\eye(dim))); 0119 0120 FF.PoseData{newState}=PoseData(newPos,Jp*Sp*Jp'+Qw,eye(dim),step+1); 0121 0122 FF.F=FF.F*Jp'; 0123 0124 |