Advertisement
OreganoHauch

sensor_graphisch

Dec 8th, 2023
804
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
MatLab 1.52 KB | None | 0 0
  1. function [x,p,t,x_berechnet,t_verrauscht] = sensor_graphisch(varargin)
  2. params = struct('epsilon',0.2,'n',100);
  3. n_vararg = numel(varargin);
  4. if mod(n_vararg,2) ~= 0
  5.     disp('Bitte Argumentepaare eingeben!')
  6.     return
  7. else
  8.     for i=1:n_vararg/2
  9.         keyword = varargin{2*i-1};
  10.         value = varargin{2*i};
  11.         params.(keyword) = value;
  12.     end
  13. end
  14. fig=figure(1); clf; xlim([-1,1]); ylim([-1,1])
  15. grid('on')
  16. hold('on')
  17. sensors = zeros(params.n,2);
  18. for i=1:params.n
  19.     title_fstring = sprintf('Setze Sensor %i (beende mit rechtem Mausklick)',i);
  20.     title(title_fstring);
  21.     [sensor_pos_x, sensor_pos_y] = ginput(1);
  22.     sel = get(fig, 'SelectionType');
  23.     if strcmpi(sel, 'alt')
  24.         break
  25.     end
  26.     sensors(i,:) = [sensor_pos_x,sensor_pos_y];
  27.     plot(sensor_pos_x, sensor_pos_y,'ob')
  28. end
  29. v = nonzeros(sensors);
  30. sensors = reshape(v,numel(v)/2,2);
  31. title('Setze Objekt');
  32. [objekt_pos_x, objekt_pos_y] = ginput(1);
  33. plot(objekt_pos_x, objekt_pos_y,'xr')
  34. objekt = [objekt_pos_x, objekt_pos_y, rand(1)];
  35. t = sensorzeit(objekt,sensors);
  36. noise = params.epsilon;
  37. t_noise = (1+noise-2*rand(size(t))*noise).*t;
  38. objekt_berechnet = lsqnonlin(@(x) sensorfunktion(objekt,sensors,t_noise),zeros(1,3));
  39. plot(objekt_berechnet(1),objekt_berechnet(2),'ro','Markersize',16);
  40. title('Berechnete Objektposition');
  41. hold('off');
  42. end
  43.  
  44. function f=sensorfunktion(x,p,t)
  45. f=sqrt( (p(:,1) - x(1)).^2 + p(:,2) - x(2)).^2 - t + x(3);
  46. end
  47.  
  48. function t=sensorzeit(x,p)
  49. t=sqrt( (p(:,1) - x(1)).^2 + p(:,2) - x(2)).^2 + x(3);
  50. end
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement