```    1/*
2One-dimensional  Kalman filter. Hidden Markov model with a real
3value as state and a real value as output. The next state is given by
4the current state plus Gaussian noise (mean 0 and variance 2 in this example)
5and the output is given by the current state plus Gaussian noise (mean
60 and variance 1 in this example).
7This example can be considered as modeling a random walk of a single continuous
8state variable with noisy observations.
9Given that at time 0 the value 2.5 was
10observed, what is the distribution of the state at time 1 (filtering problem)?
11The distribution of the state is plotted in the case of having (posterior) or
12not having the observation (prior).
13Liklihood weighing is used to condition the distribution on evidence on
14a continuous random variable (evidence with probability 0).
15CLP(R) constraints allow both sampling and weighing samples with the same
16program.
17Filtering can be used to estimate the sequence of state variables
18given a sequence of observations. Either likelihood weighting or particle
19filtering can be used for this purpose.
20From
21Islam, Muhammad Asiful, C. R. Ramakrishnan, and I. V. Ramakrishnan.
22"Inference in probabilistic logic programs with continuous random variables."
23Theory and Practice of Logic Programming 12.4-5 (2012): 505-523.
24http://arxiv.org/pdf/1112.2681v3.pdf
25Russell, S. and Norvig, P. 2010. Arficial Intelligence: A Modern Approach.
26Third Edition, Prentice Hall, Figure 15.10 page 587
27
28*/
```

?- `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

*/

```   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]]).
92% prior as in Russel and Norvig 2010, Fig 15.10
93trans_err(_,E):gaussian(E,[0,0],[[2,0],[0,2]]).
94% transition noise as in Russel and Norvig 2010, Fig 15.10
95obs_err(_,E):gaussian(E,[0,0],[[1,0],[0,1]]).
96% observation noise as in Russel and Norvig 2010, Fig 15.10
97
hist(+S:int, +Bins:int, -C:dict) is det
Plots a histogram of the density of the state at time 1 in case of no observation
```  103hist(Samples,NBins,Chart):-
104  mc_sample_arg(kf_fin(1,_O1,Y),Samples,Y,L0,[]),
105  histogram(L0,NBins,Chart,[]).```
dens_lw(+S:int, +Bins:int, -C:dict) is det
Plots the density of the state at time 1 in case of no observation (prior) and in case of observing 2.5. Observation as in Russel and Norvig 2010, Fig 15.10
```  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).```
filter_par(+S:int, -C:dict) is det
Draws a sample trajectory for 4 time points and performs particle filtering
```  124filter_par(Samples,C):-
125  sample_trajectory(4,O,St),
126  filter_par(Samples,O,St,C).```
filter_sampled_par(+S:int, -C:dict) is det
Considers a sampled trajectory for 4 time points and performs particle filtering
```  130filter_sampled_par(Samples,C):-
131  o(O),
132  st(St),
133  filter_par(Samples,O,St,C).```
filter_par(+S:int, +O:list, +St:list, -C:dict) is det
Takes observations O and true states St for 4 time points and performs filtering on the trajectory: given O, computes the distribution of the state for each time point. It uses particle filtering with S particles. Returns a graph C with the distributions of the state variable at time 1, 2, 3 and 4 (S1, S2, S3, S4, density on the left y axis) and with O and St (time on the right y axis).
```  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  }.```
filter(+S:int, -C:dict) is det
Draws a sample trajectory for 4 time points and performs filtering with likelihood weighting
```  179filter(Samples,C):-
180  sample_trajectory(4,O,St),
181  filter(Samples,O,St,C).```
filter_sampled(+S:int, -C:dict) is det
Considers a sampled trajectory for 4 time points and performs filtering with likelihood weighting
```  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]]).```
filter(+S:int, +O:list, +St:list, -C:dict) is det
Takes observations O and true states St for 4 time points and performs filtering on the trajectory: given O, computes the distribution of the state for each time point by taking S samples using likelihood weighting. Returns a graph C with the distributions of the state variable at time 1, 2, 3 and 4 (S1, S2, S3, S4, density on the left y axis) and with O and St (time on the right y axis).
```  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 % legend:_{show: false},
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  % Considers a sampled trajectory for 4 time points and performs particle filtering
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,' ')```