diff --git a/doc/doc/tutorial/08-rangeonly-slam/solution.m b/doc/doc/tutorial/08-rangeonly-slam/solution.m new file mode 100644 index 000000000..5560d2cd6 --- /dev/null +++ b/doc/doc/tutorial/08-rangeonly-slam/solution.m @@ -0,0 +1,125 @@ +% Codac - Examples +% Dynamic range-bearing localization +% ---------------------------------------------------------------------------- + +import py.codac.* + +% =========== CREATING DATA =========== + +dt = 0.05; +iteration_dt = 0.2; +tdomain = Interval(0,15); % [t0,tf] + +% Initial pose x0=(0,0,2) +x0 = [0, 0, 2]; + +% System input +u = Trajectory(tdomain, TFunction('3*(sin(t)^2)+t/100'), dt); + +% Noise +i_n = Interval(-0.03,0.03); % the noises are known to be bounded by i_n + +n_u = RandTrajectory(tdomain, dt, i_n); % input noise +n_theta = RandTrajectory(tdomain, dt, i_n); % heading noise + +% Actual trajectories (state + derivative) +v_truth = TrajectoryVector(int32(3)); +x_truth = TrajectoryVector(int32(3)); +v_truth.setitem(int32(2), u + n_u); +x_truth.setitem(int32(2), v_truth.getitem(int32(2)).primitive() + x0(3)); +v_truth.setitem(int32(0), 10*cos(x_truth.getitem(int32(2)))); +v_truth.setitem(int32(1), 10*sin(x_truth.getitem(int32(2)))); +x_truth.setitem(int32(0), v_truth.getitem(int32(0)).primitive() + x0(1)); +x_truth.setitem(int32(1), v_truth.getitem(int32(1)).primitive() + x0(2)); + +% Bounded trajectories (dead reckoning) +v = TubeVector(tdomain, dt, int32(3)); +x = TubeVector(tdomain, dt, int32(3)); +v.setitem(int32(2), Tube(u, dt).inflate(i_n.rad())); % command u with bounded uncertainties +x.setitem(int32(2), Tube(x_truth.getitem(int32(2))+n_theta, dt).inflate(i_n.rad())); % heading measurement with bounded uncertainties +v.setitem(int32(0), 10*cos(x.getitem(int32(2)))); +v.setitem(int32(1), 10*sin(x.getitem(int32(2)))); +x = v.primitive()+IntervalVector(x0); % dead reckoning + +% Set of landmarks +v_m = { py.list([6,12]), py.list([-2,-5]), py.list([-3,20]), py.list([3,4]) }; + +% =========== GRAPHICS =========== + +beginDrawing(); + +fig_map = VIBesFigMap('slam'); +fig_map.set_properties(int32(50), int32(50), int32(1200), int32(600)); +fig_map.add_tube(x, 'x', int32(0), int32(1)); +fig_map.add_trajectory(x_truth, 'truth', int32(0), int32(1), 'white'); +fig_map.smooth_tube_drawing(true); +fig_map.add_landmarks(py.list(v_m), single(0.4)); +fig_map.show(double(1)); + +% =========== CONTRACTOR NETWORK =========== + +v_m_boxes = cell(size(v_m)); +for i=1:length(v_m) + v_m_boxes(i) = {IntervalVector(int32(2))}; +end + +% Contractor Network: + +cn = ContractorNetwork(); + +t = tdomain.lb(); +prev_t_obs = t; + +while t < tdomain.ub() + + if t-prev_t_obs > 2*dt % new observation each 2*delta + + % Creating new observation to a random landmark + + landmark_id = randi([1 length(v_m)]); % a random landmark is perceived + + xt = double(x_truth(t)); + pos_x = [xt(1), xt(2)]; + pos_b = double(v_m{landmark_id}); + + yi = Interval(sqrt((pos_x(1)-pos_b(1))^2+(pos_x(2)-pos_b(2))^2)); + yi.inflate(0.03); % adding range bounded uncertainty + + prev_t_obs = t; + + % Adding related observation constraints to the network + + % Alias (for ease of reading) + b = v_m_boxes{landmark_id}; + + % Intermediate variables + ti = Interval(t); + xi = IntervalVector(int32(3)); + + % Contractors + cn.add(CtcEval(), py.list({ti, xi, x, v})); + cn.add(CtcDist(), py.list({xi.getitem(int32(0)), xi.getitem(int32(1)), b.getitem(int32(0)), b.getitem(int32(1)), yi})); + + end + + contraction_dt = cn.contract_during(iteration_dt); + if iteration_dt>contraction_dt + pause(iteration_dt-contraction_dt); % iteration delay + end + + % Display the current slice x + fig_map.draw_box(x(t).subvector(int32(0),int32(1))); + + t = t + dt; + +end + +cn.contract(true); % lets the solver run the remaining contractions + +fig_map.show(); +for i=1:length(v_m_boxes) + b = v_m_boxes{i}; + fig_map.draw_box(b); +end + +endDrawing();