47:- use_module(library(mcintyre)). 48:- if(current_predicate(use_rendering/1)). 49:- use_rendering(c3). 50:- endif. 51:- mc. 52:- begin_lpad. 53
54kf_fin(N, T) :-
55 kf_fin(N,_O,T).
56
57kf_fin(N,O, T) :-
58 init(S),
59 kf_part(0, N, S,O,_LS,T).
60
61
62kf(N,O,LS) :-
63 init(S),
64 kf_part(0, N, S,O,LS,_T).
65
66kf_o(N,ON):-
67 init(S),
68 N1 is N-1,
69 kf_part(0,N1,S,_O,_LS,T),
70 emit(T,N,ON).
71
72kf_part(I, N, S, [V|RO], [S|LS], T) :-
73 I < N,
74 NextI is I+1,
75 trans(S,I,NextS),
76 emit(NextS,I,V),
77 kf_part(NextI, N, NextS, RO, LS, T).
78
79kf_part(N, N, S, [], [], S).
80
81trans([SX,SY],I,[NextSX,NextSY]) :-
82 {NextSX =:= EX + SX},
83 {NextSY =:= EY + SY},
84 trans_err(I,[EX,EY]).
85
86emit([NextSX,NextSY],I,[VX,VY]) :-
87 {VX =:= NextSX +XX},
88 {VY =:= NextSY +XY},
89 obs_err(I,[XX,XY]).
90
91init(S):gaussian(S,[0,0],[[1,0],[0,1]]).
93trans_err(_,E):gaussian(E,[0,0],[[2,0],[0,2]]).
95obs_err(_,E):gaussian(E,[0,0],[[1,0],[0,1]]).
97
98:- end_lpad.
103hist(Samples,NBins,Chart):-
104 mc_sample_arg(kf_fin(1,_O1,Y),Samples,Y,L0,[]),
105 histogram(L0,NBins,Chart,[]).
111dens_lw(Samples,NBins,Chart):-
112 mc_sample_arg(kf_fin(1,_O1,Y),Samples,Y,L0,[]),
113 mc_lw_sample_arg(kf_fin(1,_O2,T),kf_fin(1,[2.5],_T),Samples,T,L),
114 densities(L0,L,NBins,Chart).
115
116dens_par(Samples,NBins,Chart):-
117 mc_sample_arg(kf_fin(1,_O1,Y),Samples,Y,L0,[]),
118 mc_particle_sample_arg(kf_fin(1,_O2,T),[kf_fin(1,[2.5],_T)],Samples,T,L),
119 densities(L0,L,NBins,Chart).
124filter_par(Samples,C):-
125 sample_trajectory(4,O,St),
126 filter_par(Samples,O,St,C).
130filter_sampled_par(Samples,C):-
131 o(O),
132 st(St),
133 filter_par(Samples,O,St,C).
144filter_par(Samples,O,St,C):-
145 O=[O1,O2,O3,O4],
146 NBins=20,
147 mc_particle_sample_arg([kf_fin(1,T1),kf_fin(2,T2),kf_fin(3,T3),kf_fin(4,T4)],
148 [kf_o(1,O1),kf_o(2,O2),kf_o(3,O3),kf_o(4,O4)],Samples,[T1,T2,T3,T4],[F1,F2,F3,F4]),
149 density(F1,NBins,C1,[]),
150 density(F2,NBins,C2,[]),
151 density(F3,NBins,C3,[]),
152 density(F4,NBins,C4,[]),
153 [[x|X1],[dens|S1]]=C1.data.columns,
154 [[x|X2],[dens|S2]]=C2.data.columns,
155 [[x|X3],[dens|S3]]=C3.data.columns,
156 [[x|X4],[dens|S4]]=C4.data.columns,
157 Y=[1,2,3,4],
158 C = c3{data:_{xs:_{'True State':xt,'Obs':xo,'S1':x1,'S2':x2,'S3':x3,'S4':x4},
159 columns:[[xt|St],['True State'|Y],
160 [xo|O],['Obs'|Y],
161 [x1|X1],['S1'|S1],
162 [x2|X2],['S2'|S2],
163 [x3|X3],['S3'|S3],
164 [x4|X4],['S4'|S4]],
165 types:_{'S1': spline,'S2': spline,'S3': spline,'S4': spline,'True State':scatter,'Obs':scatter},
166 axes:_{'S1':y,'S2':y,'S3':y,'S4':y,'True State':y2,'Obs':y2}},
167 axis:_{ x:_{ tick:_{fit:false}},
168 y2:_{
169 show: 'true',
170 label: 'Time',
171 min: -6
172 },
173 y:_{label:'Density'}}
174 }.
179filter(Samples,C):-
180 sample_trajectory(4,O,St),
181 filter(Samples,O,St,C).
186filter_sampled(Samples,C):-
187 o(O),
188 st(St),
189 filter(Samples,O,St,C).
190
191o([[1.6127838678200224, -1.697623495651141], [-1.3012722402069283, -0.3977302301159187], [0.24622836043423546, -2.820880242430126], [3.300910998551033, -4.691031964628459]]).
192st([[1.969626682239083, -1.798206221002788], [0.7843699589892734, -2.2118887644198812], [-1.1097437868957478, -1.5011916399908098], [1.3760932583093937, -3.00585815124235]]).
203filter(Samples,O,St,C):-
204 mc_lw_sample_arg(kf(4,_O,T),kf_fin(4,O,_T),Samples,T,L),
205 maplist(separate,L,T1,T2,T3,T4),
206 NBins=20,
207 density(T1,NBins,C1,[]),
208 density(T2,NBins,C2,[]),
209 density(T3,NBins,C3,[]),
210 density(T4,NBins,C4,[]),
211 [[x|X1],[dens|S1]]=C1.data.columns,
212 [[x|X2],[dens|S2]]=C2.data.columns,
213 [[x|X3],[dens|S3]]=C3.data.columns,
214 [[x|X4],[dens|S4]]=C4.data.columns,
215 Y=[1,2,3,4],
216 C = c3{data:_{xs:_{'True State':xt,'Obs':xo,'S1':x1,'S2':x2,'S3':x3,'S4':x4},
217 columns:[[xt|St],['True State'|Y],
218 [xo|O],['Obs'|Y],
219 [x1|X1],['S1'|S1],
220 [x2|X2],['S2'|S2],
221 [x3|X3],['S3'|S3],
222 [x4|X4],['S4'|S4]],
223 types:_{'S1': spline,'S2': spline,'S3': spline,'S4': spline,'True State':scatter,'Obs':scatter},
224 axes:_{'S1':y,'S2':y,'S3':y,'S4':y,'True State':y2,'Obs':y2}},
225 226 axis:_{ x:_{ tick:_{fit:false}},
227 y2:_{
228 show: 'true',
229 label: 'Time',
230 min: -6
231 },
232 y:_{label:'Density'}}
233 }.
234
235separate([S1,S2,S3,S4]-W,S1-W,S2-W,S3-W,S4-W).
236
237sample_trajectory(N,Ob,St):-
238 mc_sample_arg(kf(N,O,T),1,(O,T),L,[]),
239 L=[[(Ob,St)]-_].
240
241 242filter_sampled_par_dist(Samples,[D1,D2,D3,D4]):-
243 o(O),
244 filter_par_dist(Samples,O,[P1,P2,P3,P4]),
245 density2d(P1,D1,[nbins(20)]),
246 density2d(P2,D2,[nbins(20)]),
247 density2d(P3,D3,[nbins(20)]),
248 density2d(P4,D4,[nbins(20)]),
249 write_file(D1,'dist1.m'),
250 write_file(D2,'dist2.m'),
251 write_file(D3,'dist3.m'),
252 write_file(D4,'dist4.m').
253
254filter_par_dist(Samples,O,[F1,F2,F3,F4]):-
255 O=[O1,O2,O3,O4],
256 mc_particle_sample_arg([kf_fin(1,T1),kf_fin(2,T2),kf_fin(3,T3),kf_fin(4,T4)],
257 [kf_o(1,O1),kf_o(2,O2),kf_o(3,O3),kf_o(4,O4)],Samples,[T1,T2,T3,T4],[F1,F2,F3,F4]).
258
259write_file(D,Name):-
260 open(Name,write,S),
261 maplist(get_x_y_z_mat,D,X,Y,Z),
262write(S,'X = '),
263write_mat(S,X),
264write(S,'Y = '),
265write_mat(S,Y),
266write(S,'Z = '),
267write_mat(S,Z),
268write(S,"figure('Name','"),
269write(S,dist),
270writeln(S,"','NumberTitle','off');"),
271writeln(S,"surf(X,Y,Z)"),
272close(S).
273
274get_x_y_z_mat(D,X,Y,Z):-
275 maplist(get_x_y_z,D,X,Y,Z).
276
277get_x_y_z([X,Y]-Z,X,Y,Z).
278
279write_mat(S,M):-
280 writeln(S,'['),
281 append(M0,[ML],M),!,
282 maplist(write_row(S),M0),
283 maplist(write_col(S),ML),
284 nl(S),
285 writeln(S,']'),
286 nl(S).
287
288 write_row(S,R):-
289 maplist(write_col(S),R),
290 writeln(S,';').
291
292 write_col(S,E):-
293 write(S,E),
294 write(S,' ')
?-
filter_sampled_par_dist(4000,D)
. ?-filter_sampled_par(100,C)
. ?-filter_par(100,C)
. ?-filter_sampled(1000,C)
. ?-filter(1000,C)
. ?-dens_par(1000,40,G)
. ?-dens_lw(1000,40,G)
. % plot the density of the state at time 1 in case of no observation (prior) % and in case of observing 2.5 by taking 1000 samples and dividing the domain % in 40 bins ?-hist(1000,40,G)
. % plot the density of the state at time 1 in case of no observation % by taking 1000 samples and dividing the domain % in 40 bins*/