clearvars
load('training_pairs')

for i=1:length(position)
    t1 = angle_array(i,1);
    t2 = angle_array(i,2);
    t3 = angle_array(i,3);

    phi(:,i) = [t1 t2 t3 1]';
    phi2(:,i) = [t1^2 t2^2 t3^2 t1*t2 t2*t3 1]';
end

Phi = phi'
Phi2 = phi2'
alpha = Phi\position;
alpha2 =     Phi2\position;
Yls = Phi*alpha;
Yls2 = Phi2*alpha2;
immse(Yls, position)
immse(Yls2, position)

% plot x to compare
plot(1:10, position(:,1),1:10, Yls(:,1))
% plot x to compare
plot(1:10, position(:,2),1:10, Yls(:,2))
% plot y to compare
plot(1:10, position(:,3),1:10, Yls(:,3))