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*/

   46:- use_module(library(mcintyre)).   47:- if(current_predicate(use_rendering/1)).   48:- use_rendering(c3).   49:- endif.   50:- mc.   51:- begin_lpad.   52
   53kf_fin(N, T) :-
   54  kf_fin(N,_O,T).
   55
   56kf_fin(N,O, T) :-
   57  init(S),
   58  kf_part(0, N, S,O,_LS,T).
   59
   60
   61kf(N,O,LS) :-
   62  init(S),
   63  kf_part(0, N, S,O,LS,_T).
   64
   65kf_o(N,ON):-
   66  init(S),
   67  N1 is N-1,
   68  kf_part(0,N1,S,_O,_LS,T),
   69  emit(T,N,ON).
   70
   71kf_part(I, N, S, [V|RO], [S|LS], T) :-
   72  I < N, 
   73  NextI is I+1,
   74  trans(S,I,NextS),
   75  emit(NextS,I,V),
   76  kf_part(NextI, N, NextS, RO, LS, T).
   77
   78kf_part(N, N, S, [], [], S).
   79
   80trans(S,I,NextS) :-
   81  {NextS =:= E + S},
   82  trans_err(I,E).
   83
   84emit(NextS,I,V) :-
   85  {V =:= NextS +X},
   86  obs_err(I,X).
   87
   88init(S):gaussian(S,0,1).
   89% prior as in Russel and Norvig 2010, Fig 15.10
   90trans_err(_,E):gaussian(E,0,2).
   91% transition noise as in Russel and Norvig 2010, Fig 15.10
   92obs_err(_,E):gaussian(E,0,1).
   93% observation noise as in Russel and Norvig 2010, Fig 15.10
   94
   95:- end_lpad.

 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

  100hist(Samples,NBins,Chart):-
  101  mc_sample_arg(kf_fin(1,_O1,Y),Samples,Y,L0),
  102  histogram(L0,Chart,[nbins(NBins)]).

 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

  108dens_lw(Samples,NBins,Chart):-
  109  mc_sample_arg(kf_fin(1,_O1,Y),Samples,Y,L0),
  110  mc_lw_sample_arg(kf_fin(1,_O2,T),kf_fin(1,[2.5],_T),Samples,T,L),
  111  densities(L0,L,Chart,[nbins(NBins)]).
  112
  113dens_par(Samples,NBins,Chart):-
  114  mc_sample_arg(kf_fin(1,_O1,Y),Samples,Y,L0),
  115  mc_particle_sample_arg(kf_fin(1,_O2,T),[kf_fin(1,[2.5],_T)],Samples,T,L),
  116  densities(L0,L,Chart,[nbins(NBins)]).

 filter_par(+S:int, -C:dict) is det

Draws a sample trajectory for 4 time points and performs particle filtering

  121filter_par(Samples,C):-
  122  sample_trajectory(4,O,St),
  123  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

  127filter_sampled_par(Samples,C):-
  128  o(O),
  129  st(St),
  130  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).

  141filter_par(Samples,O,St,C):-
  142  O=[O1,O2,O3,O4],
  143  NBins=20,
  144  mc_particle_sample_arg([kf_fin(1,T1),kf_fin(2,T2),kf_fin(3,T3),kf_fin(4,T4)],
  145  [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]),
  146  density(F1,C1,[nbins(NBins)]),
  147  density(F2,C2,[nbins(NBins)]),
  148  density(F3,C3,[nbins(NBins)]),
  149  density(F4,C4,[nbins(NBins)]),
  150  [[x|X1],[dens|S1]]=C1.data.columns,
  151  [[x|X2],[dens|S2]]=C2.data.columns,
  152  [[x|X3],[dens|S3]]=C3.data.columns,
  153  [[x|X4],[dens|S4]]=C4.data.columns,
  154  Y=[1,2,3,4],
  155  C = c3{data:_{xs:_{'True State':xt,'Obs':xo,'S1':x1,'S2':x2,'S3':x3,'S4':x4},
  156  columns:[[xt|St],['True State'|Y],
  157    [xo|O],['Obs'|Y],
  158    [x1|X1],['S1'|S1],
  159    [x2|X2],['S2'|S2],
  160    [x3|X3],['S3'|S3],
  161    [x4|X4],['S4'|S4]],
  162    types:_{'S1': spline,'S2': spline,'S3': spline,'S4': spline,'True State':scatter,'Obs':scatter},
  163    axes:_{'S1':y,'S2':y,'S3':y,'S4':y,'True State':y2,'Obs':y2}},
  164    axis:_{ x:_{ tick:_{fit:false}},
  165      y2:_{
  166            show: 'true',
  167                label: 'Time',
  168                min: -6
  169       },
  170       y:_{label:'Density'}}
  171  }.

 filter(+S:int, -C:dict) is det

Draws a sample trajectory for 4 time points and performs filtering with likelihood weighting

  176filter(Samples,C):-
  177  sample_trajectory(4,O,St),
  178  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

  183filter_sampled(Samples,C):-
  184  o(O),
  185  st(St),
  186  filter(Samples,O,St,C).
  187
  188o([-0.13382010096024688, -1.1832019975321675, -3.2127809027386567, -4.586259511038596]).
  189st([-0.18721387460211258, -2.187978176930458, -1.5472275345566668, -2.9840114021132713]).

 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).

  200filter(Samples,O,St,C):-
  201  mc_lw_sample_arg(kf(4,_O,T),kf_fin(4,O,_T),Samples,T,L),
  202  maplist(separate,L,T1,T2,T3,T4),
  203  NBins=20,
  204  density(T1,C1,[nbins(NBins)]),
  205  density(T2,C2,[nbins(NBins)]),
  206  density(T3,C3,[nbins(NBins)]),
  207  density(T4,C4,[nbins(NBins)]),
  208  [[x|X1],[dens|S1]]=C1.data.columns,
  209  [[x|X2],[dens|S2]]=C2.data.columns,
  210  [[x|X3],[dens|S3]]=C3.data.columns,
  211  [[x|X4],[dens|S4]]=C4.data.columns,
  212  Y=[1,2,3,4],
  213  C = c3{data:_{xs:_{'True State':xt,'Obs':xo,'S1':x1,'S2':x2,'S3':x3,'S4':x4},
  214  columns:[[xt|St],['True State'|Y],
  215    [xo|O],['Obs'|Y],
  216    [x1|X1],['S1'|S1],
  217    [x2|X2],['S2'|S2],
  218    [x3|X3],['S3'|S3],
  219    [x4|X4],['S4'|S4]],
  220    types:_{'S1': spline,'S2': spline,'S3': spline,'S4': spline,'True State':scatter,'Obs':scatter},
  221    axes:_{'S1':y,'S2':y,'S3':y,'S4':y,'True State':y2,'Obs':y2}},
  222 % legend:_{show: false},
  223    axis:_{ x:_{ tick:_{fit:false}},
  224      y2:_{
  225            show: 'true',
  226                label: 'Time',
  227                min: -6
  228       },
  229       y:_{label:'Density'}}
  230  }.
  231
  232separate([S1,S2,S3,S4]-W,S1-W,S2-W,S3-W,S4-W).
  233
  234sample_trajectory(N,Ob,St):-
  235  mc_sample_arg(kf(N,O,T),1,(O,T),L),
  236  L=[[(Ob,St)]-_]