sensor_graphisch

function [x,p,t,x_berechnet,t_verrauscht] = sensor_graphisch(varargin)
params = struct('epsilon',0.2,'n',100);
n_vararg = numel(varargin);
if mod(n_vararg,2) ~= 0
    disp('Bitte Argumentepaare eingeben!')
    return
else
    for i=1:n_vararg/2
        keyword = varargin{2*i-1};
        value = varargin{2*i};
        params.(keyword) = value;
    end
end
fig=figure(1); clf; xlim([-1,1]); ylim([-1,1])
grid('on')
hold('on')
sensors = zeros(params.n,2);
for i=1:params.n
    title_fstring = sprintf('Setze Sensor %i (beende mit rechtem Mausklick)',i);
    title(title_fstring);
    [sensor_pos_x, sensor_pos_y] = ginput(1);
    sel = get(fig, 'SelectionType');
    if strcmpi(sel, 'alt')
        break
    end
    sensors(i,:) = [sensor_pos_x,sensor_pos_y];
    plot(sensor_pos_x, sensor_pos_y,'ob')
end
v = nonzeros(sensors);
sensors = reshape(v,numel(v)/2,2);
title('Setze Objekt');
[objekt_pos_x, objekt_pos_y] = ginput(1);
plot(objekt_pos_x, objekt_pos_y,'xr')
objekt = [objekt_pos_x, objekt_pos_y, rand(1)];
t = sensorzeit(objekt,sensors);
noise = params.epsilon;
t_noise = (1+noise-2*rand(size(t))*noise).*t;
objekt_berechnet = lsqnonlin(@(x) sensorfunktion(objekt,sensors,t_noise),zeros(1,3));
plot(objekt_berechnet(1),objekt_berechnet(2),'ro','Markersize',16);
title('Berechnete Objektposition');
hold('off');
end

function f=sensorfunktion(x,p,t)
f=sqrt( (p(:,1) - x(1)).^2 + p(:,2) - x(2)).^2 - t + x(3);
end

function t=sensorzeit(x,p)
t=sqrt( (p(:,1) - x(1)).^2 + p(:,2) - x(2)).^2 + x(3);
end