p_incorrect.m

来自「SLAM Gridsim use with sparer sensor」· M 代码 · 共 39 行

function [p_inc] = p_incorrect(sensor, env)

F=sensor.F;
rmax=sensor.rmax;
rho=sensor.rho;
dB=sensor.db;
sb=sensor.sb;
sr=sensor.sr;
ee=sensor.ee;
ef=sensor.ef;

delta=env.delta;
M=env.M;
d=env.d;

Ee = ((1-d)*(1-ee)+d*ef);
Ef = (d*(1-ef)+(1-d)*ee);

p_inc_e=0;
p_inc_f=0;
for r=0:ceil((rmax+sr)/delta)
    % p(no closer cells register as F)
    p_obs=1;
    if(r*delta > sr) p_obs = Ee^(dB*r^2); end
    % p(a cell within sr of r registers as F)
    p_f = min(1, Ef * (dB/delta^2)*((r*delta+sr)^2-max(0,r*delta-sr)^2));
    % p(a cell in the beam which reports range r is in C_F)
    p_incf = ((r*delta+sr)^2-max(0,r*delta-sr)^2)/(r*delta+sr)^2;
    % p(a cell in the beam which reports range r is in C_E)
    p_ince = max(0,r*delta-sr)^2/(r*delta+sr)^2;

    if(isnan(p_incf)) p_incf=0; end
    if(isnan(p_ince)) p_ince=0; end
    
    p_inc_e = p_inc_e + p_incf*p_obs*p_f;
    p_inc_f = p_inc_f + p_ince*p_obs*p_f;
end

p_inc = d*p_inc_f + (1-d)*p_inc_e;