example_num_opt_SHS_physical

function [estFl, Fl, fval, exitflag] = example_num_opt_SHS_physical
%
% Physical-parameter numerical optimization example
%

%% For testing, generate some random trajectories

% Sample once every dt seconds
dt = 5;
t = 0:dt:120;

% Noise scale
noisesd = 0.05;

% Arbitrary physical parameters (to fit later)

% Not fitting these parameters
global g mu mL Fp

% Gravity (constant)
g = 9.8;

% Coefficient of friction (estimate from empirical measurements of load)
mu = 0.1;

% Per-ant pulling force (estimate from empirical measurements)
Fp = 0.1;

% Mass of load (weigh it)
mL = 1;

% Per-ant lifting force (estimate from data)
Fl = 0.1;

% Arbitrary rates for testing
% states are (detached, pulling, lifting)
lambda = [ 0 2 3 ; 5 0 9 ; 2 7 0 ];
%lambda = lambda/max(max(lambda)) * 1/(2*dt);

% Graph laplacian (to generate fake count data)
laplacian = lambda.*(1-eye(size(lambda))) - (1-eye(size(lambda)))*lambda.*eye(size(lambda));

% Random initial condition for mean-field trajectory
meandata = zeros(length(lambda), length(t));
meandata(:, 1) = randperm(length(lambda))';

% Initial position and velocity
v = zeros(length(t),1);
x = zeros(length(t),1);

% Generate mean-field trajectories based on Laplacian and physical
% parameters
% (NOTE: This is a naive model of motion that doesn't properly treat the
%        stop-in-the-middle-of-a-sampling-interval case.)
for i = 2:length(t)
    meandata(:, i) = expm(laplacian*t(i))*meandata(:, 1);
    num_pullers = meandata(2, i);
    num_lifters = meandata(3, i);
    Fn = mL*g - sign(v(i-1))*num_lifters*Fl;
    Fg = num_pullers*Fp;
    accel = (Fg - mu*Fn)/mL;
    v(i) = v(i-1) + accel*dt + noisesd*randn;
    x(i) = x(i-1) + v(i-1)*dt + 0.5*accel*dt^2 + noisesd*randn;
end


%% Use a least-squares fit to get physical parameters back out from meandata

% fmincon (non-linear optimization with constraints) arguments:
%    1. Function handle defining the metric to minimize
%    2. Initial condition (all of the off-diagonal elements of lambda matrix)
%    3. Matrix "A" in the inequality constraint A*x <= b
%    4. Vector "b" in the inequality constraint A*x <= b
% In the linear inequality constraint, A*x <= b, "x" is a feasible solution
% to the optimization problem.

% The output vector is the optimal parameters for the fit
[estFl, fval, exitflag] = fmincon( @(fitFl)( lsmetric(fitFl, x, v, meandata, t) ), ...
            rand, ... % initial conditions
            [], [], ... % inequalities (A*x <= b)
            [], [], ... % equalities (Aeq*x == beq)
            [ 0 ], ... % lower bounds
            [ Inf ] ... % upper bounds
            );

end

%% Metric for least-squares fit

% This metric sums up the squared differences between the data (i.e.,
% meandata at times t) and a trajectory defined by "lambdavec"

function lssum = lsmetric( Fl, x, v, meandata, t )

    % Pull in globals
    global g mu mL Fp

    testv = zeros(length(t),1);
    testx = zeros(length(t),1);

    % This sums up the squared differences over time
    lssum = 0;
    dt = t(2)-t(1);
    for i = 2:length(t)
        num_pullers = meandata(2, i);
        num_lifters = meandata(3, i);
        Fn = mL*g - sign(testv(i-1))*num_lifters*Fl;
        Fg = num_pullers*Fp;
        accel = (Fg - mu*Fn)/mL;
        testv(i) = testv(i-1) + accel*dt;
        testx(i) = testx(i-1) + testv(i-1)*dt + 0.5*accel*dt^2;

        % Could also fit the velocities too...
        lssum = lssum + norm( testx(i) - x(i) )^2 + norm( testv(i) - v(i) )^2;
    end

end