Newer
Older
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
%previous_N_fixed
%N_fixed
%nb_times_fixed
%alpha_variable
%confidence
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
window_size
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.alpha_variable = [];
%obj.confidence = [];
%obj.nb_times_fixed = [];
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)];
frame = size(obj.T,1);
% estimate alpha and adjust the window size
alpha = obj.estimate_alpha();
sprintf('estimated alpha_variable: %f', alpha);
obj.current_alpha = alpha;
obj.window_size = [obj.window_size, frame];
% estimate X inside the window (and update N_fixed)
%obj.previous_N_fixed = obj.N_fixed;
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
% save the value of alpha for the frames that have converged
%if obj.previous_N_fixed < obj.N_fixed
% for i=obj.previous_N_fixed+1:obj.N_fixed
% obj.alpha_variable(i) = alpha;
% end
%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
end
% if alpha <= 0.015
% alpha = 0.015;
% end
% if alpha > 0.025
% alpha = 0.025;
% end
end
end
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.confidence = [obj.confidence, -loglikelihood / size(new_X_variable,1)];
obj.X = new_X_variable;
function spread_X(obj)
N = size(obj.X,1);
if N ==1
obj.X = 0;
%obj.confidence = 1;
obj.current_alpha = obj.alpha_initial;
obj.plot_estimated_X()
obj.compute_loglikelihood_dynamics(1:N)
%obj.confidence = [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;
%obj.confidence = [1, -loglikelihood / size(new_X,1), 1];
%%% 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
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
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.alpha_initial, 0, 2, 1, obj.alpha_initial, obj.sigma_test);
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
function X = normalise_X(obj, X)
X(X<0) = 0;
X(X>1) = 1;
end
function periodic = is_periodic(obj)
periodic = 0;
end
end
%methods (Abstract)
% X = normalise_X(obj, X)
% periodic = is_periodic(obj)
%end
end