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

   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
   98:- 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

  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,' ')