observation_model.m
function [ zi ] = observation_model( orientation, position, fi )
zi=orientation'*(fi-position);
end
function [ zi ] = observation_model( orientation, position, fi )
zi=orientation'*(fi-position);
end