1+ import py.codac4matlab.*
2+
3+ % [B-q3-beg]
4+ M = {IntervalVector([1.5,2.5]),IntervalVector([3,1]), IntervalVector([2,2]), IntervalVector([2.5,3]), IntervalVector([3.5,2]), IntervalVector([4,1]), IntervalVector([1.5,0.5])};
5+
6+ for i = 1:numel(M)
7+ M{i}.inflate(0.05);
8+ end
9+
10+ a1 = IntervalVector({{1.25,3.},{1.6,2.75}});
11+ a2 = IntervalVector({{2,3.5},{0.6,1.2}});
12+ a3 = IntervalVector({{1.1,3.25},{0.2,1.4}});
13+
14+ ctc_constell = MyCtc(M);
15+
16+ [a1,a2,a3] = deal(ctc_constell.contract(a1),ctc_constell.contract(a2),ctc_constell.contract(a3))
17+ % [B-q3-end]
18+
19+ % [B-q4-beg]
20+ x_truth = Vector([2,1,PI/6]);
21+ % [B-q4-end]
22+
23+ % [B-q5-beg]
24+ DefaultFigure().draw_tank(x_truth, 0.4, StyleProperties({Color().black(),Color().yellow()}))
25+ for i = 1:numel(M)
26+ DefaultFigure().draw_box(M{i}, StyleProperties({Color().dark_green(),Color().green()}))
27+ end
28+
29+ DefaultFigure().set_axes(axis(1,Interval([1.,4.5])), axis(2,Interval([0.,3.5])));
30+ % [B-q5-end]
31+
32+ % [B-q6-beg]
33+ function p = g(x, mi)
34+ r = py.codac4matlab.sqrt(py.codac4matlab.sqr(mi(1)-x(1))+py.codac4matlab.sqr(mi(2)-x(2)));
35+ b = py.codac4matlab.atan2(mi(2)-x(2),mi(1)-x(1)) - x(3);
36+ p = py.codac4matlab.IntervalVector({r,b}).inflate(0.02);
37+ end
38+ % [B-q6-end]
39+
40+ % [B-q7-beg]
41+ obs = {};
42+ for i = 1:numel(M)
43+ obs{end+1} = cart_prod(g(x_truth, M{i}), M{i});
44+ end
45+
46+ for i = 1:numel(obs)
47+ DefaultFigure().draw_pie(x_truth.subvector(1,2),obs{i}(1), x_truth(3)+obs{i}(2),Color().red());
48+ DefaultFigure().draw_pie(x_truth.subvector(1,2),obs{i}(1).union(0),x_truth(3)+obs{i}(2),Color().gray());
49+ end
50+ % [B-q7-end]
51+
52+ ctc_polar = CtcPolar();
53+
54+ [x1,x2,x3] = deal(VectorVar(2), VectorVar(3), VectorVar(2));
55+ f_minus = AnalyticFunction({x1,x2,x3},vec(x1(1)-x2(1)-x3(1), x1(2)-x2(2)-x3(2)));
56+ ctc_minus = CtcInverse(f_minus, Vector([0,0]));
57+
58+ [s1,s2,s3] = deal(ScalarVar(),ScalarVar(),ScalarVar());
59+ f_plus = AnalyticFunction({s1,s2,s3}, s1+s2-s3);
60+ ctc_plus = CtcInverse(f_plus, Interval(0,0));
61+
62+ % [B-q7b-beg]
63+ function [x,yi,mi,ai,di] = ctc_one_obs(x,yi,mi,ai,di,ctc_plus,ctc_polar,ctc_minus)
64+ res_ctc_minus = ctc_minus.contract(py.codac4matlab.cart_prod(mi,x,di)); % The result is a 7D IntervalVector
65+ [mi,x,di] = deal(res_ctc_minus.subvector(1,2),res_ctc_minus.subvector(3,5),res_ctc_minus.subvector(6,7));
66+
67+ res_ctc_plus = ctc_plus.contract(py.codac4matlab.cart_prod(x(3), yi(2), ai)); % The result is a 3D IntervalVector
68+ x.setitem(3,res_ctc_plus(1));
69+ yi.setitem(2,res_ctc_plus(2));
70+ ai = res_ctc_plus(3);
71+
72+ res_ctc_polar = ctc_polar.contract(py.codac4matlab.cart_prod(di(1),di(2),yi(1),ai)); % The result is a 4D IntervalVector
73+ di.setitem(1,res_ctc_polar(1));
74+ di.setitem(2,res_ctc_polar(2));
75+ yi.setitem(1,res_ctc_polar(3));
76+ ai = res_ctc_polar(4);
77+ end
78+
79+ function [x,y,m,a,d] = fixpoint_ctc_one_obs(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus)
80+ vol = -1.;
81+ prev_vol = -2.;
82+
83+ while vol ~= prev_vol
84+ prev_vol = vol;
85+ [x,y,m,a,d] = ctc_one_obs(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus);
86+ vol = x.volume() + y.volume() + m.volume() + a.volume() + d.volume();
87+ end
88+ end
89+
90+ function x = ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus)
91+ for i = 1:numel(obs)
92+ yi = obs{i};
93+ ai = py.codac4matlab.Interval();
94+ di = py.codac4matlab.IntervalVector(2);
95+ mi = yi.subvector(3,4);
96+ [x,yi,mi,ai,di] = fixpoint_ctc_one_obs(x,yi,mi,ai,di,ctc_plus,ctc_polar,ctc_minus)
97+ end
98+ end
99+
100+ function x = fixpoint_ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus)
101+ vol = -1.;
102+ prev_vol = -2.;
103+
104+ while vol ~= prev_vol
105+ prev_vol = vol;
106+ x = ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus);
107+ vol = x.volume();
108+ end
109+ end
110+
111+ x = cart_prod(IntervalVector(2),x_truth(3));
112+ x = fixpoint_ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus);
113+ % [B-q7b-end]
114+
115+ % [B-q8-beg]
116+ x
117+ DefaultFigure().draw_box(x)
118+ % [B-q8-end]
119+
120+ % [B-q9-beg]
121+ function [x,yi,mi,ai,di] = ctc_one_obs_datasso(x,yi,mi,ai,di,ctc_plus,ctc_polar,ctc_minus,ctc_constell)
122+ res_ctc_minus = ctc_minus.contract(py.codac4matlab.cart_prod(mi,x,di)); % The result is a 7D IntervalVector
123+ [mi,x,di] = deal(res_ctc_minus.subvector(1,2),res_ctc_minus.subvector(3,5),res_ctc_minus.subvector(6,7));
124+
125+ res_ctc_plus = ctc_plus.contract(py.codac4matlab.cart_prod(x(3), yi(2), ai)); % The result is a 3D IntervalVector
126+ x.setitem(3,res_ctc_plus(1));
127+ yi.setitem(2,res_ctc_plus(2));
128+ ai = res_ctc_plus(3);
129+
130+ res_ctc_polar = ctc_polar.contract(py.codac4matlab.cart_prod(di(1),di(2),yi(1),ai)); % The result is a 4D IntervalVector
131+ di.setitem(1,res_ctc_polar(1));
132+ di.setitem(2,res_ctc_polar(2));
133+ yi.setitem(1,res_ctc_polar(3));
134+ ai = res_ctc_polar(4);
135+ % ==== Added contractor ====
136+ mi = ctc_constell.contract(mi);
137+ % ==========================
138+ end
139+
140+ function [x,y,m,a,d] = fixpoint_ctc_one_obs_datasso(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus)
141+ vol = -1.;
142+ prev_vol = -2.;
143+
144+ while vol ~= prev_vol
145+ prev_vol = vol;
146+ [x,y,m,a,d] = ctc_one_obs(x,y,m,a,d,ctc_plus,ctc_polar,ctc_minus);
147+ vol = x.volume() + y.volume() + m.volume() + a.volume() + d.volume();
148+ end
149+ end
150+
151+ function x = ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus)
152+ for i = 1:numel(obs)
153+ yi = obs{i};
154+ ai = py.codac4matlab.Interval();
155+ di = py.codac4matlab.IntervalVector(2);
156+ mi = yi.subvector(3,4);
157+ [x,yi,mi,ai,di] = fixpoint_ctc_one_obs(x,yi,mi,ai,di,ctc_plus,ctc_polar,ctc_minus)
158+ end
159+ end
160+
161+ function x = fixpoint_ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus)
162+ vol = -1.;
163+ prev_vol = -2.;
164+
165+ while vol ~= prev_vol
166+ prev_vol = vol;
167+ x = ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus);
168+ vol = x.volume();
169+ end
170+ end
171+
172+ x = cart_prod(IntervalVector(2),x_truth(3));
173+ x = fixpoint_ctc_all_obs(x,obs,ctc_plus,ctc_polar,ctc_minus);
0 commit comments