Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
classdef Dynamics_Model < matlab.mixin.Copyable %handle
properties
num_dim_Y
interpolant_XY
sigma_estimation
sigma_test
alpha_initial
initial_estimate_X
X
T
Y
current_alpha
display_screen
marginal_X_Yi
limits_Yi
end
methods
function obj = Dynamics_Model(filename_interpolant, display_screen)
obj.display_screen = display_screen;
if obj.display_screen ~= -1
figure(display_screen)
clf(display_screen)
end
structure = load(filename_interpolant);
obj.interpolant_XY = structure.interpolant_XY;
obj.num_dim_Y = size(structure.interpolant_Y.GridVectors,2);
N = size(structure.interpolant_Y.GridVectors{1}, 2);
for i=1:obj.num_dim_Y
obj.limits_Yi(i,1) = structure.interpolant_Y.GridVectors{i}(1);
obj.limits_Yi(i,2) = structure.interpolant_Y.GridVectors{i}(N);
end
for i=1:obj.num_dim_Y
obj.marginal_X_Yi(:,:,i) = flipdim(structure.P_Yi_knowing_X(:,:,i),1);
end
end
function initialise(obj, initial_X, alpha_ini, sigma_estimation, sigma_test)
obj.T = [];
obj.X = [];
obj.Y = [];
obj.current_alpha = alpha_ini;
obj.initial_estimate_X = initial_X;
obj.alpha_initial = alpha_ini;
obj.sigma_estimation = sigma_estimation;
obj.sigma_test = sigma_test;
end
function process_new_frame(obj, Yt, Tt)
obj.T = [obj.T; Tt];
obj.Y = [obj.Y; Yt(:,1:obj.num_dim_Y)];
% estimate alpha and adjust the window size
alpha = obj.estimate_alpha();
sprintf('estimated alpha_variable: %f', alpha);
obj.current_alpha = alpha;
obj.estimate_X();
%sprintf('"normalised" log likelihood of X (or confidence): %f', obj.confidence(frame));
if obj.display_screen ~= -1
obj.plot_estimated_X();
end
end
function alpha = estimate_alpha(obj)
N = size(obj.X,1);
alpha = obj.alpha_initial;
return
end
first_index = 1;
nb_points = N;
if nb_points < 3 % we need at least 2 intervals to compute the mean
alpha = obj.alpha_initial;
else
X_2_to_N = obj.X(first_index+1:N);
X_1_to_Nminus1 = obj.X(first_index:N-1);
T_2_to_N = obj.T(first_index+1:N);
T_1_to_Nminus1 = obj.T(first_index:N-1);
delta_X = X_2_to_N - X_1_to_Nminus1;
delta_T = T_2_to_N - T_1_to_Nminus1;
factors = delta_X ./ delta_T;
alpha = mean(factors);
if alpha <= 0.85 * obj.alpha_initial; %0.75 %0.015 0.75%0.85%0.9
alpha = 0.85 * obj.alpha_initial; %0.75%0.015;%0.85%0.9
if alpha >= 1.15 * obj.alpha_initial; %1.25%0.015 1.25%1.15%1.1
alpha = 1.15 * obj.alpha_initial; %1.25%0.015;%1.15%1.1
function estimate_X(obj)
%function to be minimised over new_X, with extra parameters Y, alpha and T
f = @(variable_X)(-obj.logP_x_knowing_y_paquet(variable_X,obj.current_alpha,obj.sigma_estimation));
N = size(obj.X,1);
if N == 0
X_ini = obj.initial_estimate_X;
X_ini = [obj.X; obj.X(N) + obj.current_alpha * (obj.T(N+1) - obj.T(N))];
lb = repmat(0,1,length(X_ini));
ub = repmat(1,1,length(X_ini));
A = [];
b = [];
Aeq = [];
beq = [];
[new_X_variable,loglikelihood] = fmincon(f,X_ini,A,b,Aeq,beq,lb,ub);
obj.X = new_X_variable;
function spread_X(obj)
N = size(obj.X,1);
if N ==1
obj.X = 0;
obj.current_alpha = obj.alpha_initial;
obj.plot_estimated_X()
obj.compute_loglikelihood_dynamics(1:N)
return
elseif N == 2
obj.X = [0 1];
obj.current_alpha = 1;
obj.plot_estimated_X()
obj.compute_loglikelihood_dynamics(1:N)
%%% estimation initiale de X
%%% calcul du alpha correspondant
X_2_to_N = X_tmp(2:N)';
X_1_to_Nminus1 = X_tmp(1:N-1)';
T_2_to_N = obj.T(2:N);
T_1_to_Nminus1 = obj.T(1:N-1);
delta_X = X_2_to_N - X_1_to_Nminus1;
delta_T = T_2_to_N - T_1_to_Nminus1;
factors = delta_X ./ delta_T;
alpha = mean(factors);
%function to be minimised over new_X, with extra parameters Y, alpha and T
f = @(variable_X)(-obj.logP_x_knowing_y_spread(variable_X,alpha,obj.sigma_estimation));
%%% estimation de X
lb = repmat(0,1,length(X_ini));
ub = repmat(1,1,length(X_ini));
A = [];
b = [];
Aeq = [];
beq = [];
[new_X,loglikelihood] = fmincon(f,X_ini,A,b,Aeq,beq,lb,ub);
new_X = [0; obj.normalise_X(new_X); 1];
obj.X = new_X;
%%% calcul du alpha correspondant aux X estimés
X_2_to_N = obj.X(2:N);
X_1_to_Nminus1 = obj.X(1:N-1);
T_2_to_N = obj.T(2:N);
T_1_to_Nminus1 = obj.T(1:N-1);
delta_X = X_2_to_N - X_1_to_Nminus1;
delta_T = T_2_to_N - T_1_to_Nminus1;
factors = delta_X ./ delta_T;
obj.current_alpha = mean(factors);
adeline.paiement
committed
if obj.current_alpha <= 0.85 * obj.alpha_initial; %0.75 %0.015 0.75%0.85%0.9
obj.current_alpha = 0.85 * obj.alpha_initial; %0.75%0.015;%0.85%0.9
adeline.paiement
committed
end
if obj.current_alpha >= 1.15 * obj.alpha_initial; %1.25%0.015 1.25%1.15%1.1
obj.current_alpha = 1.15 * obj.alpha_initial; %1.25%0.015;%1.15%1.1
adeline.paiement
committed
end
obj.compute_loglikelihood_dynamics(1:N)
function loglikelihood = logP_x_knowing_y_paquet(obj, variable_X, alpha, sigma)
N_variable = size(variable_X,1);
% if N == 1, we maximise P_cond_y(Y(1),X(1)) * P_x_Markov_simple(X(1),X(0)) avec X(0) virtuel
% if N == 2, we maximise P_cond_y(Y(2),X(2)) * P_x_Markov_simple(X(2),X(1)) * P_cond_y(Y(1),X(1)) with P_x_Markov_simple(X(2),X(1)) = 0 if delta_X is negative, and uniform proba otherwise
loglikelihood = obj.logP_cond_y(obj.Y(1,:),variable_X(1));
loglikelihood = loglikelihood + obj.logP_x_Markov(variable_X(1), variable_X(1)-alpha, 2, 1, alpha, sigma);
Y_2_to_N = obj.Y(2:N_variable,:);
X_2_to_N = variable_X(2:N_variable);
X_1_to_Nminus1 = variable_X(1:N_variable-1);
T_2_to_N = obj.T(2:N_variable);
T_1_to_Nminus1 = obj.T(1:N_variable-1);
ll_proba_cond_y = obj.logP_cond_y(Y_2_to_N, X_2_to_N);
ll_proba_x_Markov = obj.logP_x_Markov(X_2_to_N,X_1_to_Nminus1,T_2_to_N,T_1_to_Nminus1,alpha,sigma);
vect_tmp = ll_proba_cond_y + ll_proba_x_Markov;
loglikelihood = loglikelihood + sum(vect_tmp);
end
end
function loglikelihood = logP_x_knowing_y_spread(obj, variable_X, alpha, sigma)
N_variable = size(variable_X,1);
% we maximise P_cond_y(Y(1),0) * P_x_Markov_simple(X(2),0) * P_cond_y(Y(i),X(i)) * P_x_Markov_simple(X(i+1),X(i)) * P_x_Markov_simple(1,X(N-1)) * P_cond_y(Y(N),1)
loglikelihood = obj.logP_cond_y(obj.Y(1,:),0) + obj.logP_x_Markov(variable_X(1),0,obj.T(2),obj.T(1),alpha,sigma);
loglikelihood = loglikelihood + sum(obj.logP_cond_y(obj.Y(2:end-1,:), variable_X));
loglikelihood = loglikelihood + obj.logP_x_Markov(1, variable_X(end),obj.T(end),obj.T(end-1),alpha,sigma);
loglikelihood = loglikelihood + obj.logP_cond_y(obj.Y(end,:),1);
if N_variable > 1
ll_proba_x_Markov = obj.logP_x_Markov(variable_X(2:end),variable_X(1:end-1),obj.T(3:end-1),obj.T(2:end-2),alpha,sigma);
loglikelihood = loglikelihood + sum(ll_proba_x_Markov);
end
end
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
function loglikelihood = logP_cond_y(obj, Yi, Xi)
s = size(Yi,1);
comp_small = repmat(obj.limits_Yi(:,1)',s,1);
comp_big = repmat(obj.limits_Yi(:,2)',s,1);
too_small = Yi < comp_small;
too_big = Yi > comp_big;
Yi(too_small) = comp_small(too_small);
Yi(too_big) = comp_big(too_big);
xy = [obj.normalise_X(Xi) Yi];
if isempty(xy)
loglikelihood = [];
return;
end
loglikelihood = log(obj.interpolant_XY(xy));
end
function loglikelihood = logP_x_Markov(obj, Xi, Xim, Ti, Tim, alpha, sigma)
difference = (Xi - Xim) - alpha .* (Ti - Tim);
% penalise negative and null dX
indexes = Xi - Xim < 0;
if length(alpha) > 1
difference(indexes) = 5 * (Xi(indexes) - Xim(indexes)) - alpha(indexes) .* (Ti(indexes) - Tim(indexes));
else
difference(indexes) = 5 * (Xi(indexes) - Xim(indexes)) - alpha .* (Ti(indexes) - Tim(indexes));
end
if isempty(difference)
loglikelihood = -100;
else
loglikelihood = log(normpdf(difference,0,sigma));

Laurie Boffelli
committed
index2 = loglikelihood < -100;%a été supprimé

Laurie Boffelli
committed
loglikelihood(index2) = -100;%a été supprimé
function llh_dynamics = compute_loglikelihood_dynamics(obj, frames)
%llh_dynamics(frames <= 1) = nan;
indexes = frames > 1 & frames <= length(obj.T);
alpha = obj.current_alpha;
terme1 = obj.logP_cond_y(obj.Y(frames(indexes),:), obj.X(frames(indexes)));
terme2 = obj.logP_x_Markov(obj.X(frames(indexes)),obj.X(frames(indexes)-1),obj.T(frames(indexes)),obj.T(frames(indexes)-1),alpha,obj.sigma_test);
llh_dynamics(indexes) = terme1 + terme2;
indexes = frames <= 1
frames(indexes)
terme1 = obj.logP_cond_y(obj.Y(frames(indexes),:), obj.X(frames(indexes)));
llh_dynamics(frames <= 1) = terme1 + obj.logP_x_Markov(obj.X(1), obj.X(1)-alpha, 2, 1, alpha, obj.sigma_test);
end
function plot_estimated_X(obj)
if obj.display_screen ~= -1
N = size(obj.X,1);
start_frame = max(N - 50, 1);
figure(obj.display_screen)
for i=1:obj.num_dim_Y
subplot(obj.num_dim_Y,1,i)
hold off
imagesc([0 1],[obj.limits_Yi(i,1) obj.limits_Yi(i,2)],obj.marginal_X_Yi(:,:,i),'CDataMapping','scaled')
set(gca,'YDir','normal')
title(['Likelihood of (X,Y_' i ') to be normal (color plot) and estimated (X,Y_' i ') (black crosses)'])
xlabel('Percentage of movement completion X')
ylabel(['Manifold coordinate Y_' i])
hold on
plot(obj.normalise_X(obj.X(start_frame:N)),obj.Y(start_frame:N,i),'kx')
end
%pour faire des gifs
%saveas(obj.display_screen, strcat('estimatedX_frame', int2str(N), '.png'))
end
end
function [im]=final_plot_estimated_X(obj)
if obj.display_screen ~= -1
N = size(obj.X,1);
start_frame = max(N - 50, 1);
figure(obj.display_screen)
for i=1:obj.num_dim_Y
subplot(obj.num_dim_Y,1,i)
hold off
im=imagesc([0 1],[obj.limits_Yi(i,1) obj.limits_Yi(i,2)],obj.marginal_X_Yi(:,:,i),'CDataMapping','scaled')
%set(gca,'YDir','normal')
hold on
plot(obj.normalise_X(obj.X(start_frame:N)),obj.Y(start_frame:N,i),'kx')
axis off
end
%pour faire des gifs
%saveas(obj.display_screen, strcat('estimatedX_frame', int2str(N), '.png'))
end
end