measurements_prediction.m

来自「移动机器人同时定位与地图创建最前沿技术」· M 代码 · 共 22 行

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% SLAM Summer School 2006, Oxford.
% Practical 3. SLAM using Monocular Vision.
% Practical exercise.
% J.M.M. Montiel, Javier Civera, Andrew J. Davison.
% {josemari, jcivera}@unizar.es, [e-mail]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [ h_all, H_predicted, S ] = measurements_prediction( x_km1_k, p_km1_k, sigma_imageNoise, cam )
% Measurement prediction

[ h_all, H_predicted ] = h( x_km1_k, cam );

% Matrix S
numPredictedFeatures = size( find( h_all(:,1) > 0 ), 1 );

if numPredictedFeatures>0
    R_predicted = eye( 2*numPredictedFeatures )*sigma_imageNoise^2;
    S = H_predicted*p_km1_k*H_predicted' + R_predicted; 
else
    S = [];
end