text:
%%%%%%%%%%%%% - USV autonomous control algorithm using MPC - %%%%%%%%%%%%%%
%%% This code use and MPC algorithm to track an angle reference while   %%%
%%% keeping a constant cruise speed.                                    %%%
%%% Written by Colin Calas at Cardiff University on August 2017         %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%% TODO :
%%%     - Add the cruise velocity to the MPC
%%%     - Test with a testbench path

%Clean the workspace and close the open figures
clear
close all

%Boat and simulation parameters
m = 37;         %Mass of the boat
D = 0.7;        %Distance between the motors and the center of mass
I = 0.1;        %Moment of inertia      (arbitrary value, should be identified)
k = 0.1;        %Viscosity coefficient  (arbitrary value, should be identified)
ky = 0.1;       %Viscosity coefficient on the y axis (arbitrary value, should be identified)
Tfinal = 150;   %Total simulation time
Te = 0.1;       %Sampling period

%Vectors used to hold simulation data
x = zeros(9, ceil(Tfinal/Te));          %State vector
u = zeros(2, ceil(Tfinal/Te));          %Input vector
delta_u = zeros(2, ceil(Tfinal/Te));    %Input increment vector
a = zeros(3, ceil(Tfinal/Te));          %State vector
a_ref = zeros(1, ceil(Tfinal/Te));
i = 1;                                  %Loop index

%Ordered list of waypoints
x_list = [2 4 32 40 25 10 2]';  %X coordinates of the waypoints
y_list = [2 15 17 7 0 -5 2]';   %Y coordinates of the waypoints
a_list = zeros(7,1);            %Angle of the boat between two successive waypoints
current_obj = 2;                %As the boat starts in the first waypoint, the current objective is the next
                                %ie. the second waypoint

%Compute all the angles between two successive waypoints
%The angles returned are between -pi and pi
for j=1:7
   a_list(mod(j,7)+1,1) = angle(complex(x_list(mod(j,7)+1)-x_list(j), y_list(mod(j,7)+1)-y_list(j)));

   if a_list(j,1) < 0
      a_list(j,1) = a_list(j,1);
   end
end

%Objectives list containing X,Y and Theta coordinates
r_list = [x_list y_list a_list];
nb_obj = size(r_list,1);                %Number of objectives

%MPC horizons
nu = 2;         %Control horizon (dof of the optimization problem)
ny = 30;        %Prediction horizon

%State-space system used for the MPC
a(:,i) = [0 0 1.4181]';                 %Initial conditions : the boat is in the correct orientation
                                        %And has null angular speed and acceleration
u(:,i) = [k/2 k/2]';                    %Initial condition on the command : to maintain a speed of 1m/s
delta_u(:,i) = [0 0]';                  %Initial condition on the input increments

%System matrices
A = [1  0 0;...                         %State matrix
     Te 1 0;...
     0 Te 1];
B = [D/(2*I) -D/(2*I); 0 0; 0 0];       %Input matrix

%Augmented system matrices
A_tilde = [A B; zeros(2,3), eye(2)];    %State matrix
B_tilde = [zeros(3,2); eye(2)];         %Input matrix
C_tilde = [0 0 1 0 0];                  %Output matrix
n_output = size(C_tilde,1);                    %Dimension of the output
n_input = size(B,2);            %Number of inputs
n_state = size(B,1);            %Number of state variables

%State-space system used to provide an angle reference vector
x(:,i) = [2 2 1.4181 0 0 1 0 0 0]';         %State initial condition : the boat is in the first waypoint
                                        %and correctly oriented
Fx = [1 0 0 0 0 Te*cos(x(3,i)) 0 -Te*sin(x(3,i)) 0;...    %Linearized state matrix
      0 1 0 0 0 Te*sin(x(3,i)) 0 Te*cos(x(3,i)) 0;...
      0 0 1 Te 0 0 0 0 0;...
      0 0 0 1 Te 0 0 0 0;...
      0 0 0 0 1  0 0 0 0;...
      0 0 0 0 0  1 Te 0 0;...
      0 0 0 0 0 -k/m 1 0 0;...
      0 0 0 0 0 0 0 1 Te;...
      0 0 0 0 0 0 0 -ky/m 1];
Fu = [0 0; 0 0; 0 0; 0 0;...            %Linearized input matrix
      D/(2*I) -D/(2*I); 0 0; 1/m 1/m; 0 0; 0 0];

Dx = -0.02;
Dy = 0.01;
alpha = angle(complex(Dx,Dy));
Dk = [0 0 0 0 0 0 (1/m)*Dx*cos(alpha-x(3,i)) 0 (1/m)*Dy*sin(alpha-x(3,i))]';

r = zeros(n_output*ny,1);                      %Angle reference vector

%Matrices used for optimization
gainS = computeGainS(n_output, 1);             %Steady-state gain
gainQ = computeGainQ(n_output, 1);             %Running cost gain on the output
gainR = computeGainR(2, 1);             %Running cost gain on the input
Qbar = computeQbar(C_tilde, gainQ, gainS, ny);
Tbar = computeTbar(gainQ, C_tilde, gainS, ny);
Rbar = computeRbar(gainR, nu);
Cbar = computeCbar(A_tilde, B_tilde, ny, nu);
Abar = computeAbar(A_tilde, ny);
Hbar = computeHbar(Cbar, Qbar, Rbar);
Fbar = computeFbar(Abar, Qbar, Cbar, Tbar);

%Quadprog solver options
%Keep the solver quiet
options = optimoptions('quadprog');
options = optimoptions(options, 'Display', 'off');

for t=0:Te:(Tfinal-1)
    a_tilde = [a(:,i); u(:,i)]; %Build the augmented state space vector for MPC

    %Compute the ny next angle objectives
    tmp = x(:,i);           %Get the current state
    obj = current_obj;      %Get the current objective

    j = 1;
    while(j <= ny)
        %Compute the angle of the line between the boat and the next waypoint
        goal_angle = angle(complex(r_list(obj,1)-tmp(1,1), r_list(obj,2)-tmp(2,1)));

        %If the distance between the goal angle and the current angle is
        %greater than pi, it means that there is a shorter path to this angle
        %than getting all the way around the unit circle
        dist = abs(goal_angle - tmp(3,1));
        if dist > pi
            if tmp(3,1) < 0
                goal_angle = tmp(3,1) - (2*pi - dist);
            else
                goal_angle = tmp(3,1) + (2*pi - dist);
            end
        end

        %Considering an optimal orientation of the boat and equals inputs
        %compute the Ny next boat position.
        %If the boat is not yet arrived to the waypoint put the goal_angle into
        %the reference vector, else recompute the goal angle.
        tmp(3,1) = goal_angle;
        Fx = [1 0 0 0 0 Te*cos(tmp(3,1)) 0 -Te*sin(tmp(3,1)) 0;...    %Linearized state matrix
              0 1 0 0 0 Te*sin(tmp(3,1)) 0 Te*cos(tmp(3,1)) 0;...
              0 0 1 Te 0 0 0 0 0;...
              0 0 0 1 Te 0 0 0 0;...
              0 0 0 0 1  0 0 0 0;...
              0 0 0 0 0  1 Te 0 0;...
              0 0 0 0 0 -k/m 1 0 0;...
              0 0 0 0 0 0 0 1 Te;...
              0 0 0 0 0 0 0 -ky/m 1];
        Dk = [0 0 0 0 0 0 (1/m)*Dx*cos(alpha-tmp(3,1)) 0 (1/m)*Dy*sin(alpha-tmp(3,1))]';
        tmp = Fx*tmp + Fu*[k/2;k/2] + Dk;    %Make a step forward

        %Have we reached the next waypoint ?
        if(tmp(1) > r_list(obj,1)-0.5 && tmp(1) < r_list(obj,1)+0.5 && ...
           tmp(2) > r_list(obj,2) - 0.5 && tmp(2) < r_list(obj,2) + 0.5)

            %If yes, recompute the goal angle and update the jacobian
            obj = mod(obj,7) + 1;
            goal_angle = angle(complex(r_list(obj,1)-tmp(1,1), r_list(obj,2)-tmp(2,1)));
            dist = abs(goal_angle - tmp(3,1));
            if dist > pi
                if tmp(3,1) < 0
                    goal_angle = tmp(3,1) - (2*pi - dist);
                else
                    goal_angle = tmp(3,1) + (2*pi - dist);
                end
            end

            tmp(3,1) = goal_angle;
            Fx = [1 0 0 0 0 Te*cos(tmp(3,1)) 0 -Te*sin(tmp(3,1)) 0;...    %Linearized state matrix
                  0 1 0 0 0 Te*sin(tmp(3,1)) 0 Te*cos(tmp(3,1)) 0;...
                  0 0 1 Te 0 0 0 0 0;...
                  0 0 0 1 Te 0 0 0 0;...
                  0 0 0 0 1  0 0 0 0;...
                  0 0 0 0 0  1 Te 0 0;...
                  0 0 0 0 0 -k/m 1 0 0;...
                  0 0 0 0 0 0 0 1 Te;...
                  0 0 0 0 0 0 0 -ky/m 1];
            Dk = [0 0 0 0 0 0 (1/m)*Dx*cos(alpha-tmp(3,1)) 0 (1/m)*Dy*sin(alpha-tmp(3,1))]';
        end
        r(j,1) = goal_angle;
        j = j + 1;
    end

    a_ref(1,i) = r(1,1);

    %Constraints related matrices
    Acons = computeCy(ny,n_state+n_input)*Cbar;
    Bcons = computeDy([-100 -100 -2*pi 1*k/2 1*k/2]', [100 100 2*pi 1.2*k/2 1.2*k/2]', ny) - ...
                      computeCy(ny,n_state+n_input)*Abar*a_tilde;

    %Solve the optimization problem, if impossible to solve : break and
    %display the results
    [delta_uFut, fval, exitflag] = quadprog(Hbar/2+Hbar'/2,([a_tilde; r]'*Fbar)',Acons,Bcons,[],[],[],[],[],options);
    if(exitflag == -2)
        break;
    end

    delta_u(:,i) = delta_uFut([1 2],1);     %Get the input increment to apply
    u(:,i) = u(:,i) + delta_u(:,i);         %Compute the input
    a(:,i+1) = A*a(:,i) + B*u(:,i);         %Compute the next state for the MPC state-space
    x(3,i) = a(3,i);                        %Make the angles equal between both systems
    Fx = [1 0 0 0 0 Te*cos(x(3,i)) 0 -Te*sin(x(3,i)) 0;...    %Linearized state matrix
          0 1 0 0 0 Te*sin(x(3,i)) 0 Te*cos(x(3,i)) 0;...
          0 0 1 Te 0 0 0 0 0;...
          0 0 0 1 Te 0 0 0 0;...
          0 0 0 0 1  0 0 0 0;...
          0 0 0 0 0  1 Te 0 0;...
          0 0 0 0 0 -k/m 1 0 0;...
          0 0 0 0 0 0 0 1 Te;...
          0 0 0 0 0 0 0 -ky/m 1];
    Dk = [0 0 0 0 0 0 (1/m)*Dx*cos(alpha-x(3,i)) 0 (1/m)*Dy*sin(alpha-x(3,i))]';
    x(:,i+1) = Fx*x(:,i)+ Fu*u(:,i) + Dk;        %Compute the next state for the position system

    i = i + 1;

    %Had we past the current waypoint
    if (x(1,i) < r_list(current_obj,1) + 1 && x(1,i) > r_list(current_obj,1) - 1) && ...
       (x(2,i) < r_list(current_obj,2) + 1 && x(2,i) > r_list(current_obj,2) - 1)

        %If the current objective was the last : godd job, we're done
        %Break and display the results
        if current_obj == nb_obj
            break;
        end

        %Else increment the current_obj
        current_obj = current_obj + 1;
    end

end

%Plot the boat trajectory
subplot(4,3,[1 4]);
plot(x(1,1:i), x(2,1:i), 'k');
l = 1;
h = 1.5;
for j=1:i
   if mod(j,20) == 0
        line([x(1,j)-(h/2)*cos(x(3,j))+(l/2)*sin(x(3,j)) x(1,j)-(h/2)*cos(x(3,j))-(l/2)*sin(x(3,j))],...
             [x(2,j)-(h/2)*sin(x(3,j))-(l/2)*cos(x(3,j)) x(2,j)-(h/2)*sin(x(3,j))+(l/2)*cos(x(3,j))]);
        line([x(1,j)-(h/2)*cos(x(3,j))+(l/2)*sin(x(3,j)) x(1,j)+(h/2)*cos(x(3,j))],...
             [x(2,j)-(h/2)*sin(x(3,j))-(l/2)*cos(x(3,j)) x(2,j)+(h/2)*sin(x(3,j))]);
        line([x(1,j)-(h/2)*cos(x(3,j))-(l/2)*sin(x(3,j)) x(1,j)+(h/2)*cos(x(3,j))],...
             [x(2,j)-(h/2)*sin(x(3,j))+(l/2)*cos(x(3,j)) x(2,j)+(h/2)*sin(x(3,j))]);
   end
end
%Plot the waypoints circles of acceptance
hold on
theta = 0:0.01:2*pi;
rayon = 1;
X = rayon*cos(theta);
Y = rayon*sin(theta);
for j=1:7
    plot(X + r_list(j,1),Y + r_list(j,2), 'r')
end
quiver(20,5,Dx,Dy,800, 'm-', 'LineWidth',1);
hold off
axis([-1 43 -8 20]);

%Plot the first input
subplot(4,3,2);
plot(1:i, u(1,1:i), 'b');
axis([0 round(i,-3) 0 0.08]);

%Plot the second input
subplot(4,3,5);
plot(1:i, u(2,1:i), 'r');
axis([0 round(i,-3) 0 0.08]);

%Plot the angle from the "x" state vector
subplot(4,3,[7 10]);
plot(1:i, x(3,1:i));

%Plot the velocity
subplot(4,3,[3 6]);
plot(1:i, x(6,1:i));
axis([0 round(i,-3) 0 1.5]);

%Plot the angle from the "a" state vector
subplot(4,3,[8 11]);
plot(1:i, a(3,1:i));
hold on
plot(1:i, a_ref(1,1:i));
hold off

subplot(4,3,[9 12]);
plot(1:i, x(8,1:i));

author: ccalas

repo: mpc

save_path: github-repos/MATLAB/ccalas-mpc


text:
function phototrans = GetPhotoTrans(photostate)
% 4 photostates: 0 = all black; 1 = black/white; 2 = white/black; 3 = all white;
phototrans = zeros(size(photostate));
sw_ = find([1 diff(photostate)]);
interval = mode(diff(sw_));
sw = sw_(2):interval:length(photostate);
if sw_(2)-sw_(1)>interval,
    sw = [1, sw_(2)-interval, sw];
else
    sw = [1, sw];
end
sw_last = [length(photostate)-interval+1,sw]; % fake-circular
sw_end = [sw(2:end),length(photostate)+1]; % fake-circular
for k = 1:length(sw),
    i = photostate(sw_last(k)); % last digit
    j = photostate(sw(k)); % current digit
    phototrans(sw(k):sw_end(k)-1) = i*4 + j;
end

end

author: xiuyechen

repo: FishExplorer


text:
classdef ODE23Integrator < AbstractFirstOrderIntegrator
    %ODE23Integrator Summary of this class goes here
    %   Detailed explanation goes here

    properties
        options(1,1) BuiltInIntegratorOptions = BuiltInIntegratorOptions();
        integratorEnum = IntegratorEnum.ODE23;
    end

    methods
        function obj = ODE23Integrator(options)
            if(nargin > 0)
                obj.options = options;
            else
                obj.options = BuiltInIntegratorOptions();
            end
        end

        function [t,y,te,ye,ie] = integrate(obj, odefun, tspan, y0, evtsFunc, odeOutputFun)
            odeSetOptions = obj.options.getIntegratorOptions();
            optionsToUse = odeset(odeSetOptions, 'Events',evtsFunc, 'OutputFcn',odeOutputFun);

            [t,y,te,ye,ie] = ode23(odefun, tspan, y0, optionsToUse);
        end

        function options = getOptions(obj)
            options = obj.options;
        end
    end
end

author: Arrowstar

repo: ksptot

save_path: github-repos/MATLAB/Arrowstar-ksptot


text:
function mom = cpot_to_mpot(can)
% CPOT_TO_MPOT Convert a canonical potential to moment form.
% mom = cpot_to_mpot(can)

[logp, mu, Sigma] = canonical_to_moment(can.g, can.h, can.K);
mom = mpot(can.domain, can.sizes, logp, mu, Sigma);

%%%%%%%

function [logp, mu, Sigma] = canonical_to_moment(g, h, K)
% CANONICAL_TO_MOMENT Convert canonical characteristics to moment form.
% [logp, mu, Sigma] = canonical_to_moment(g, h, K)

n = length(K);
if isempty(K)
  logp = g - 0.5*(log(det(K)) - n*log(2*pi));
  Sigma = [];
  mu = [];
else
  if det(K)==0
    Sigma = inf*ones(n,n);
    mu = zeros(n,1); % if the precision is zero, the mean is arbitrary
    logp = g; % the scaling factor for the uniform distribution is 1
  else
    Sigma = inv(K);
    mu = Sigma*h;
    logp = g - 0.5*(log(det(K)) - n*log(2*pi) - mu'*K*mu);
  end
end

author: bayesnet

repo: bnt

save_path: github-repos/MATLAB/bayesnet-bnt


text:
function [DataMat, ChannelMat] = in_data_erplab(DataFile)
% IN_DATA_ERPLAB: Imports an ERPLab file.
%
% USAGE: [DataMat, ChannelMat] = in_data_erplab(DataFile);

% @=============================================================================
% This function is part of the Brainstorm software:
% https://neuroimage.usc.edu/brainstorm
%
% Copyright (c) University of Southern California & McGill University
% This software is distributed under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPLv3
% license can be found at http://www.gnu.org/copyleft/gpl.html.
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% For more information type "brainstorm license" at command prompt.
% =============================================================================@
%
% Authors: Francois Tadel, 2017

% Load file
ErpMat = load(DataFile, '-mat');
if ~isfield(ErpMat, 'ERP')
    error('Not a valid ERPLab file: Missing .ERP field.');
end

% ===== READ ERP =====
% Initialize output matrix
nChannels = size(ErpMat.ERP.bindata, 1);
nBins     = size(ErpMat.ERP.bindata, 3);
DataMat = repmat(db_template('DataMat'), 1, nBins);
% Loop on bins
for i = 1:nBins
    % Average
    DataMat(i).F           = ErpMat.ERP.bindata(:,:,i) * 1e-6;
    DataMat(i).Time        = ErpMat.ERP.times / 1000;
    DataMat(i).Comment     = ErpMat.ERP.bindescr{i};
    DataMat(i).ChannelFlag = ones(nChannels, 1);
    DataMat(i).nAvg        = ErpMat.ERP.ntrials.accepted(i);
    DataMat(i).Device      = 'ERPLab';
    % Error
    if isfield(ErpMat.ERP, 'binerror') && ~isempty(ErpMat.ERP.binerror)
        DataMat(i).Std = ErpMat.ERP.binerror(:,:,i) * 1e-6;
    end
    % History
    DataMat(i) = bst_history('add', DataMat(i), 'erplab', ErpMat.ERP.history(:)');
end

% ===== CHANNEL FILE =====
ChannelMat = db_template('channelmat');
ChannelMat.Channel = repmat(db_template('channeldesc'), 1, nChannels);
for i = 1:nChannels
    ChannelMat.Channel(i).Type    = 'EEG';
    ChannelMat.Channel(i).Name    = ErpMat.ERP.chanlocs(i).labels;
    if isfield(ErpMat.ERP.chanlocs(i), 'X') && ~isempty(ErpMat.ERP.chanlocs(i).X)
        ChannelMat.Channel(i).Loc = [ErpMat.ERP.chanlocs(i).X; ErpMat.ERP.chanlocs(i).Y; ErpMat.ERP.chanlocs(i).Z] ./ 1000;
    end
    ChannelMat.Channel(i).Orient  = [];
    ChannelMat.Channel(i).Comment = '';
    ChannelMat.Channel(i).Weight  = 1;
end

author: brainstorm-tools


text:
function A = catstruct(varargin)
% CATSTRUCT - concatenate structures
%
%   X = CATSTRUCT(S1,S2,S3,...) concates the structures S1, S2, ... into one
%   structure X.
%
%   Example:
%     A.name = 'Me' ;
%     B.income = 99999 ;
%     X = catstruct(A,B)
%     % -> X.name = 'Me' ;
%     %    X.income = 99999 ;
%
%   CATSTRUCT(S1,S2,'sorted') will sort the fieldnames alphabetically.
%
%   If a fieldname occurs more than once in the argument list, only the last
%   occurence is used, and the fields are alphabetically sorted.
%
%   To sort the fieldnames of a structure A use:
%     A = CATSTRUCT(A,'sorted') ;
%
%   To concatenate two similar array of structs use simple concatenation:
%     A = dir('*.mat') ; B = dir('*.m') ; C = [A ; B] ;
%
%   When there is nothing to concatenate, the result will be an empty
%   struct (0x0 struct array with no fields).
%
%   See also CAT, STRUCT, FIELDNAMES, STRUCT2CELL

% for Matlab R13 and up
% version 2.2 (oct 2008)
% (c) Jos van der Geest
% email: jos@jasen.nl

% History
% Created:  2005
% Revisions
%   2.0 (sep 2007) removed bug when dealing with fields containing cell
%                  arrays (Thanks to Rene Willemink)
%   2.1 (sep 2008) added warning and error identifiers
%   2.2 (oct 2008) fixed error when dealing with empty structs (Thanks to
%                  Lars Barring)

N = nargin ;

error(nargchk(1,Inf,N)) ;

if ~isstruct(varargin{end}),
    if isequal(varargin{end},'sorted'),
        sorted = 1 ;
        N = N-1 ;
        if N < 1,
            A = struct([]) ;
            return
        end
    else
        error('catstruct:InvalidArgument','Last argument should be a structure, or the string "sorted".') ;
    end
else
    sorted = 0 ;
end

FN = cell(N,1) ;
VAL = cell(N,1) ;

for ii=1:N,
    X = varargin{ii} ;
    if ~isstruct(X),
        error('catstruct:InvalidArgument',['Argument #' num2str(ii) ' is not a structure.']) ;
    end
    if ~isempty(X),
        % empty structs are ignored
        FN{ii} = fieldnames(X) ;
        VAL{ii} = struct2cell(X) ;
    end
end

FN = cat(1,FN{:}) ;
VAL = cat(1,VAL{:}) ;
[UFN,ind] = unique(FN) ;

if numel(UFN) ~= numel(FN),
    warning('catstruct:DuplicatesFound','Duplicate fieldnames found. Last value is used and fields are sorted') ;
    sorted = 1 ;
end

if sorted,
    VAL = VAL(ind) ;
    FN = FN(ind) ;
end

if ~isempty(FN),
    % This deals correctly with cell arrays
    A = cell2struct(VAL, FN);
else
    A = struct([]) ;
end

repo: REST


text:
% FPAT
% Schwarz, Shoelson & Amphlett
% CSSM / 2004

% let us create two objects
        objA=zeros(10,9);
        objA(4:6,6:8)=magic(3);

        objB={
              '....v....x....v....x....v' % row 1: col finder
              'a good BRETT'              % row 2
              'a bad BrrrRETt'            % row 3
              'a wide B R E T T'          % row 4
              'a shaky B RET  T'          % row 5
              'a noisy bbB rRxy EeTTtTt'  % row 6
              'a wrapped BR'              % row 7
              'ET                   '     % row 8
              'T'                         % row 9
        };

% ...and some search templates
        tmplA=magic(3);
        tmplX='BRETT';
        tmplY='B RE';
        tmplZ='B R E';

% search for patterns
        resA=fpat(tmplA,objA,'-s');
        resX=fpat(tmplX,objB);
        resY=fpat(tmplY,objB);
        resZ=fpat(tmplZ,objB,'-mrg');

% ...and show matches
        resX
        resY
        resZ
        rowZ=resZ.row
        colZ=resZ.col
        resA
        parX=resX.par

author: Sable

repo: mcbench-benchmarks


text:
function [hdr, dat] = read_nihonkohden_m00(filename)

% READ_NIHONKOHDEN_M00 reads the header and data from a file in the Nihon Kohden *.m00 format.
% This implementation is an adaptation of convert_nkascii2mat.m and get_nkheader.m written
% by Timothy Ellmore, see https://openwetware.org/wiki/Beauchamp:AnalyzeEEGinMatlab.
%
% Use as
%   [hdr, dat] = read_nihonkohden_m00(filename)
%
% This returns a FieldTrip compatible header structure and the data matrix.
%
% See also FT_READ_HEADER, FT_READ_DATA

% Copyright (C) 2007, Timothy.Ellmore@uth.tmc.edu
% Copyright (C) 2016, Diego Lozano-Soldevilla
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

% open the ascii file
fid = fopen_or_error(filename, 'r');

%header line 1: acquisition information
hline1 = fgets(fid);

% get the six different fields in the header, store in cell-array
hd = textscan(hline1, '%s %s %s %s %s %s');
fprintf('\nNihon Kohden ASCII EEG header fields:');
fprintf('\n-------------------------------------');

% number of timepoints
[txt, ntpoints] = strread(char(hd{1}), '%s%d', 'delimiter', '=');
fprintf('\n%s is %d', char(txt), ntpoints);

% number of channels sampled
[txt, nchannels] = strread(char(hd{2}), '%s%d', 'delimiter', '=');
fprintf('\nNumber of %s is %d', char(txt), nchannels);

% begin sweep in ms
[txt, bsweep] = strread(char(hd{3}), '%s%f', 'delimiter', '=');
fprintf('\n%s is %2.2f', char(txt), bsweep);

% sampling interval in ms
[txt, sampintms] = strread(char(hd{4}), '%s%f', 'delimiter', '=');
fprintf('\n%s is %1.2f (or %2.1f Hz)', char(txt), sampintms, (1000./sampintms));

% bins per micro volt
[txt, binsuV] = strread(char(hd{5}), '%s%f', 'delimiter', '=');
fprintf('\n%s is %1.2f', char(txt), binsuV);

% start time
tt = char(hd{6});
start_time = tt(end-7:end);
fprintf('\nStart Time is %s\n', start_time);

% header line 2: names of recording channels
hline2 = fgets(fid);

% channel names as cell-array
ch_names = textscan(hline2, '%s');

% convert it into a FieldTrip header
hdr.Fs          = 1000./sampintms;
hdr.nChans      = nchannels;
hdr.nSamples    = ntpoints;
hdr.nSamplesPre = 0;
hdr.nTrials     = 1;
hdr.label       = ch_names{1};

% keep the original header details
hdr.orig.hline1     = hline1;
hdr.orig.hline2     = hline2;
hdr.orig.binsuV     = binsuV;
hdr.orig.bsweep     = bsweep;
hdr.orig.ntpoints   = ntpoints;
hdr.orig.sampintms  = sampintms;
hdr.orig.start_time = start_time;

if nargout>1
  % read the remaining data
  dat = textscan(fid, '%f', hdr.nChans*hdr.nSamples);
  dat = reshape(dat{1}, hdr.nChans, hdr.nSamples);
  % calibrate the data to obtain uV values
  dat = dat ./ hdr.orig.binsuV;
end

% close input file
fclose(fid);

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
function [S,T]=sensifcn(G,Gc,H)
if nargin<=2, H=1; end
S=feedback(1,Gc*G*H); T=1-S;


text:
function showboundary(node,elem,expr,varargin)
%% SHOWBOUNDARY boundary mesh plot
%
%    showboundary(node,elem) displays the boundary surface mesh of a
%    3-dimensional tetrahderon mesh given by node and elem matrices. The
%    boundary is found by the function findboundary3.
%
%    showboundary(node,elem,expr) displays the boundary of parts of the
%    mesh specificed by the expression. For example,
%    showboundary3(node,elem,'~(x>=0 & y>=0 & z>=0)') only shows the boundary
%    mesh for tetrahedrons not in the first quadrant.
%
%    showboundary(node,elem,expr,viewangle) changes the display angle. The
%    deault view angle is view(3).
%
%    showboundary(node,elem,expr,'param','value','param','value'...)
%    allows additional patch param/value pairs to be used when displaying
%    the mesh. For example, the default transparency parameter is set to
%    0.5. You can overwrite this value by using the param pair
%    ('FaceAlpha', value). The value has to be a number between 0 and 1.
%    Other parameters include: 'Facecolor', 'Edgecolor' etc.
%
%    To display the tetrahedron mesh in 3-d, use showmesh3. Notice that the
%    3-d graphical visulization is slow for large mesh data.
%
%   Example:
%     node = [-1,-1,-1; 1,-1,-1; 1,1,-1; -1,1,-1; -1,-1,1; 1,-1,1; 1,1,1; -1,1,1];
%     elem = [1,2,3,7; 1,6,2,7; 1,5,6,7; 1,8,5,7; 1,4,8,7; 1,3,4,7];
%     [node,elem] = uniformbisect3(node,elem);
%     subplot(1,2,1);
%     showboundary3(node,elem);
%     subplot(1,2,2);
%     showboundary3(node,elem,'~(x>=0 & y>=0 & z>=0)',[59,20]);
%
%   See also showboundary, showsolution3, showmesh.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if (nargin >= 3) && (any(expr))
    x = node(:,1);  y = node(:,2);  z = node(:,3); %#ok<*NASGU>
    incl = find(eval(expr));
    elem = elem(any(ismember(elem,incl),2),:);
end
[bdNode, bdFace] = findboundary(elem); %#ok<*ASGLU>
if isempty(varargin)
    showmesh(node,bdFace);
else
    showmesh(node,bdFace,varargin{1:end});
end

repo: Aero-matlab


text:
function sbfmf_write_index(fp,framesloc,indexlocptr)

indexloc = ftell(fp);
fwrite(fp,uint64(framesloc),'uint64');
fseek(fp,indexlocptr,'bof');
fwrite(fp,uint64(indexloc),'uint64');

author: kristinbranson

repo: JAABA


text:
clear all;
close all;
more off;
addpath('tools');

epsilon = 1e-5;

x1 = [1.1 0.9 1]';
x2 = [2.2 1.85 1.2]';
z  = [0.9 1.1 1.05]';

% get the analytic Jacobian
[e, A, B] = linearize_pose_pose_constraint(x1, x2, z);

% check the error vector
e_true = [-1.06617  -1.18076  -0.85000]';
if (norm(e - e_true) > epsilon)
  disp('Your error function seems to return a wrong value');
  disp('Result of your function'); disp(e);
  disp('True value'); disp(e_true);
else
  disp('The computation of the error vector appears to be correct');
end

% compute it numerically
delta = 1e-6;
scalar = 1 / (2*delta);

% test for x1
ANumeric = zeros(3,3);
for d = 1:3
  curX = x1;
  curX(d) += delta;
  err = linearize_pose_pose_constraint(curX, x2, z);
  curX = x1;
  curX(d) -= delta;
  err -= linearize_pose_pose_constraint(curX, x2, z);

  ANumeric(:, d) = scalar * err;
end

diff = ANumeric - A;
if max(max(abs(diff))) > epsilon
  disp('Error in the Jacobian for x1');
  disp('Your analytic Jacobian'); disp(A);
  disp('Numerically computed Jacobian'); disp(ANumeric);
  disp('Difference'); disp(diff);
else
  disp('Jacobian for x1 appears to be correct');
end

% test for x2
BNumeric = zeros(3,3);
for d = 1:3
  curX = x2;
  curX(d) += delta;
  err = linearize_pose_pose_constraint(x1, curX, z);
  curX = x2;
  curX(d) -= delta;
  err -= linearize_pose_pose_constraint(x1, curX, z);

  BNumeric(:, d) = scalar * err;
end

diff = BNumeric - B;
if max(max(abs(diff))) > epsilon
  disp('Error in the Jacobian for x2');
  disp('Your analytic Jacobian'); disp(B);
  disp('Numerically computed Jacobian'); disp(BNumeric);
  disp('Difference'); disp(diff);
else
  disp('Jacobian for x2 appears to be correct');
end

author: kiran-mohan

repo: SLAM-Algorithms-Octave


text:
function spm_dcm_fmri_csd_results(DCM,action,fig)
% Review an estimated DCM for BOLD CSD
% FORMAT spm_dcm_fmri_csd_results(DCM,action,fig)
%
% Action:
%     'Spectral data'
%     'Coupling (A)'
%     'Coupling (C)'
%     'Inputs'
%     'Outputs'
%     'Transfer functions'
%     'Cross-spectra (BOLD)'
%     'Cross-spectra (neural)'
%     'Coherence (neural)'
%     'Covariance (neural)'
%     'Kernels'
%     'Functional connectivity'
%     'Location of regions'
%     'Quit'
%__________________________________________________________________________
% Copyright (C) 2013-2018 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_dcm_fmri_csd_results.m 7497 2018-11-24 17:00:25Z karl $

%-Input arguments
%==========================================================================

%-Get DCM structure
%--------------------------------------------------------------------------
if nargin < 1
    [DCM, sts] = spm_select(1,'^DCM.*\.mat$','select DCM_???.mat');
    if ~sts, return; end
end
if ~isstruct(DCM)
    load(DCM);
end

%-Get action if necessary
%--------------------------------------------------------------------------
if nargin < 2 || isempty(action)
    str = {'Timeseries data',...
        'Spectral data',...
        'Coupling (A)',...
        'Coupling (C)',...
        'Inputs',...
        'Transfer functions',...
        'Cross-spectra (BOLD)',...
        'Cross-spectra (neural)',...
        'Coherence (neural)',...
        'Covariance (neural)',...
        'Kernels',...
        'Functional connectivity',...
        'Location of regions',...
        'Quit'};

    action = str{spm_input('review',1,'m',str)};
end

%-Get figure
%--------------------------------------------------------------------------
if nargin < 3 || isempty(fig)
    Fgraph = spm_figure('GetWin','Graphics');
else
    Fgraph = fig;
    spm_figure('Focus',Fgraph);
end
colormap(gray)
if ~strcmpi(action,'Quit')
    spm_figure('Clear',Fgraph);
end

%-CSD data
%--------------------------------------------------------------------------
xY   = DCM.Y;                  % data
Hz   = DCM.Hz;                 % frequencies
name = {DCM.xY.name};          % names
ns   = size(DCM.a,1);          % number of regions
nu   = size(DCM.c,2);          % number of exogenous inputs
U    = 0.9;                    % p-value threshold for display
ns   = min(ns,8);              % bounded number of regions

%-Exponentiate coupling parameters if a two-state model
%--------------------------------------------------------------------------
try
    if DCM.options.two_state
        Ep.A = full(exp(DCM.Ep.A));
        Ep.B = full(exp(DCM.Ep.B));
        Ep.D = full(exp(DCM.Ep.D));
        Ep.C = full(DCM.Ep.C);
        disp('NB: The (A,B,D) parameters are scale parameters')
    else
        Ep.A = full(DCM.Ep.A);
        Ep.B = full(DCM.Ep.B);
        Ep.D = full(DCM.Ep.D);
        Ep.C = full(DCM.Ep.C);
    end
end

% switch
%--------------------------------------------------------------------------
switch lower(action)

    %======================================================================
    % Timeseries data
    %======================================================================
    case lower('Timeseries data')

        % graph
        %------------------------------------------------------------------
        x     = (1:DCM.v)*DCM.Y.dt;
        for i = 1:ns
            subplot(ns,1,i);
            plot(x,DCM.Y.y(:,i));
            title([DCM.Y.name{i} ': responses'],'FontSize',16);
            xlabel('time {seconds}');
        end

    %======================================================================
    % spectral data
    %======================================================================
    case lower('Spectral data')

        for i = 1:ns
            for j = i:ns
                subplot(ns,ns,(i - 1)*ns + j),cla
                plot(Hz,real(xY.csd(:,i,j))), hold on
                plot(Hz,imag(xY.csd(:,i,j)),':'), hold on
                title(sprintf('%s to %s',name{j},name{i}))
                axis square, spm_axis tight
            end

            % spectral density
            %--------------------------------------------------------------
            subplot(2,2,3)
            csd(:,i) = abs(xY.csd(:,i,i));
        end

        % spectral density
        %------------------------------------------------------------------
        subplot(2,2,3)
        plot(Hz,csd)
        title('Spectral density over modes')
        xlabel('Frequency (Hz)')
        ylabel('CSD')
        axis square, spm_axis tight
        legend(name),legend('boxoff')

        subplot(ns,ns,ns)
        legend('real','imag'),legend('boxoff')

    %======================================================================
    % Location of regions
    %======================================================================
    case lower('Location of regions')

        % graph
        %------------------------------------------------------------------
        subplot(2,1,1)
        spm_dcm_graph(DCM.xY);
        title('Regional locations','FontSize',16)

        % table
        %------------------------------------------------------------------
        subplot(2,1,2)
        y = 0;
        line([0 4],[y y])
        y = y - 1;
        text(0.0,y,'Name',         'FontSize',14)
        text(1.2,y,'Voxels',       'FontSize',14)
        text(2.0,y,'Location (mm)','FontSize',14)
        y = y - 1;
        line([0 4],[y y],'LineWidth',4)
        y = y - 1;
        for i = 1:length(DCM.xY)
            N    = size(DCM.xY(i).XYZmm, 2);
            L    = DCM.xY(i).xyz;
            r    = DCM.xY(i).spec;
            text(0.0,y,name{i}, 'FontWeight','bold',        'FontSize',12)
            text(1.5,y,sprintf('%0.0f',N),               'FontSize',12)
            text(2.0,y,sprintf('%-4.0f %-4.0f %-4.0f',L),'FontSize',12)
            y = y - 1;
        end
        line([0 4],[y y])
        axis off square

    %======================================================================
    % Inputs
    %======================================================================
    case lower('Inputs')

        % inputs
        %------------------------------------------------------------------
        try
            t     = (1:length(DCM.U.u))*DCM.U.dt;
            for i = 1:nu

                subplot(nu,1,i)
                plot(t,full(DCM.U.u(:,i)))
                title(['Input - ' DCM.U.name{i}],'FontSize',16)
                ylabel('event density {Hz}')
                spm_axis tight

            end
            xlabel('time {seconds}')
        end

    %======================================================================
    % Parameter estimates (A)
    %======================================================================
    case lower('Coupling (A)')

        % intrinsic effects
        %------------------------------------------------------------------
        subplot(2,1,1)
        a(:,:,1) = DCM.Pp.A;
        a(:,:,2) = Ep.A;
        spm_dcm_display(DCM.xY,a)
        title(sprintf('%s P(coupling > 0)','fixed'),'FontSize',16)

        % intrinsic interactions
        %------------------------------------------------------------------
        subplot(2,2,3)
        bar(a(:,:,2))
        title('A - fixed effects','FontSize',16)
        set(gca,'XTick',(1:ns),'XTickLabel',name)
        xlabel('target region')
        ylabel('strength (Hz)')
        legend(name),legend('boxoff')

        % intrinsic interactions - probabilities
        %------------------------------------------------------------------
        subplot(2,2,4)
        P = a(:,:,1);
        bar(P), hold on, plot([0 (length(P) + 1)],[U U],'-.r'), hold off
        title('A - probability','FontSize',16)
        set(gca,'XTick',1:ns,'XTickLabel',name)
        xlabel('target region')
        ylabel(sprintf('P(A > %0.2f)',0))

    %======================================================================
    % Parameter estimates (C)
    %======================================================================
    case lower('Coupling (C)')

        if spm_length(Ep.C)

            % intrinsic effects
            %--------------------------------------------------------------
            subplot(2,1,1)
            c(:,:,1) = DCM.Pp.C;
            c(:,:,2) = Ep.C;
            spm_dcm_display(DCM.xY,[],c)
            title(sprintf('%s P(coupling > 0)','fixed'),'FontSize',16)

            % intrinsic interactions
            %--------------------------------------------------------------
            subplot(2,2,3)
            bar(c(:,:,2))
            title('C - exogenous effects','FontSize',16)
            set(gca,'XTick',(1:ns),'XTickLabel',name)
            xlabel('target region')
            ylabel('strength (Hz)')
            legend(DCM.U.name),legend('boxoff')

            % intrinsic interactions - probabilities
            %--------------------------------------------------------------
            subplot(2,2,4)
            bar(c(:,:,1))
            title('C - probability','FontSize',16)
            set(gca,'XTick',(1:ns),'XTickLabel',name)
            xlabel('target region')
            ylabel(sprintf('P(C > %0.2f)',0))

        end

    %======================================================================
    % Transfer functions and cross spectra
    %======================================================================
    case lower('Transfer functions')

        for i = 1:ns
            for j = 1:ns

                subplot(ns,ns,(i - 1)*ns + j)
                dtf = abs(DCM.dtf(:,i,j));
                plot(Hz,dtf), hold on
                title(sprintf('Spectral transfer: %s to %s',name{j},name{i}))
                xlabel('frequency Hz')
                axis square, spm_axis tight

            end
        end

    %======================================================================
    % Transfer functions and cross spectra
    %======================================================================
    case lower('Cross-spectra (BOLD)')

        Gc    = DCM.Hc;                     % predicted
        Rc    = Gc + DCM.Rc;                % observed
        for i = 1:ns
            for j = i:ns
                subplot(ns,ns,(i - 1)*ns + j)
                plot(Hz,real(Gc(:,i,j)),'r'),  hold on
                plot(Hz,imag(Gc(:,i,j)),'c')
                plot(Hz,real(Rc(:,i,j)),'r:')
                plot(Hz,imag(Rc(:,i,j)),'c:'), hold off
                title(sprintf('CSD: %s to %s',name{j},name{i}))
                xlabel('frequency Hz')
                axis square, spm_axis tight
            end

            % spectral density
            %--------------------------------------------------------------
            Hc(:,i) = abs(Gc(:,i,i));
            Yc(:,i) = abs(Rc(:,i,i));

        end

        subplot(ns,ns,ns)
        legend({'pred - real','pred - imag','obs - real','obs - imag'})
        legend('boxoff')

        subplot(2,2,3)
        plot(Hz,Hc), hold on
        plot(Hz,Yc,':'), hold off
        title({'Spectral density (BOLD)'},'FontSize',16)
        xlabel('frequency (Hz)')
        ylabel('abs(CSD)')
        axis square, spm_axis tight
        legend(name),legend('boxoff')

    %======================================================================
    % Cross-spectra (neural)
    %======================================================================
    case lower('Cross-spectra (neural)')

        for i = 1:ns
            for j = i:ns
                subplot(ns,ns,(i - 1)*ns + j)
                plot(Hz,real(DCM.Hs(:,i,j)),'r'), hold on
                plot(Hz,imag(DCM.Hs(:,i,j)),'c'), hold off
                title(sprintf('CSD: %s to %s',name{j},name{i}))
                xlabel('frequency Hz')
                axis square, spm_axis tight

            end

            % spectral density
            %--------------------------------------------------------------
            Hs(:,i) = abs(DCM.Hs(:,i,i));

        end
        subplot(ns,ns,ns)
        legend({'real','imag'})
        legend('boxoff')

        subplot(2,2,3)
        plot(Hz,Hs), hold on
        title({'Spectral density (neural)'},'FontSize',16)
        xlabel('frequency (Hz)')
        ylabel('abs(CSD)')
        axis square, spm_axis tight
        legend(name),legend('boxoff')

    %======================================================================
    % Coherence (neural)
    %======================================================================
    case lower('Coherence (neural)')

        for i = 1:ns
            for j = 1:ns

                % coherence
                %---------------------------------------------------------
                if j > i
                    subplot(ns,ns,(i - 1)*ns + j),cla
                    plot(Hz,DCM.coh(:,i,j))
                    title(sprintf('Coh: %s to %s',name{j},name{i}))
                    xlabel('frequency Hz')
                    axis square, spm_axis tight
                end

                % delays
                %----------------------------------------------------------
                if j < i
                    subplot(ns,ns,(i - 1)*ns + j),cla
                    plot(Hz,DCM.fsd(:,i,j))
                    title(sprintf('Delay (s) %s to %s',name{j},name{i}))
                    xlabel('Frequency Hz')
                    axis square, spm_axis tight
                end

            end
        end

    %======================================================================
    % Cross covariance functions
    %======================================================================
    case lower('Covariance (neural)')

        for i = 1:ns
            for j = i:ns

                subplot(ns,ns,(i - 1)*ns + j),cla
                plot(DCM.pst,DCM.ccf(:,i,j))
                title(sprintf('%s to %s',name{j},name{i}))
                xlabel('lag (s)')
                axis tight square

            end

            % cross covariance functions
            %--------------------------------------------------------------
            ccf(:,i) = DCM.ccf(:,i,i);

        end

        % cross covariance functions
        %--------------------------------------------------------------
        subplot(2,2,3)
        plot(DCM.pst,ccf)
        title({'Auto-covariance';'(neural)'},'FontSize',16)
        xlabel('lag (s)')
        ylabel('auto-covariance')
        axis square

    %======================================================================
    % Functional connectivity
    %======================================================================
    case lower('Functional connectivity')

        % predicted responses
        %------------------------------------------------------------------
        [Gy,Hz,S,Gu,Gn] = spm_csd_fmri_mtf(DCM.Ep,DCM.M,DCM.U);

        % spectral density
        %------------------------------------------------------------------
        for i = 1:ns
            Yc(:,i) = abs(DCM.Y.csd(:,i,i));          % observed
            Hc(:,i) = abs(Gy(:,i,i));                 % predicted
            Nn(:,i) = abs(Gn(:,i,i));                 % Non-neuronal
            Gc(:,i) = abs(Gy(:,i,i) - Gn(:,i,i));     % Neuronal
        end

        % zero-lag correlations (functional connectivity)
        %------------------------------------------------------------------
        subplot(2,4,1)
        ccf = spm_csd2ccf(DCM.Y.csd,Hz,DCM.Y.dt);
        ccf = ccf((size(ccf,1) + 1)/2,:,:);
        cc  = spm_cov2corr(squeeze(ccf));
        image(cc*64), title('Correlations','FontSize',16), axis square

        subplot(2,4,2)
        ccf = spm_csd2ccf(Gy,Hz,DCM.Y.dt);
        ccf = ccf((size(ccf,1) + 1)/2,:,:);
        cc  = spm_cov2corr(squeeze(ccf));
        image(cc*64), title('Predicted','FontSize',16), axis square

        subplot(2,4,3)
        ccf = spm_csd2ccf(Gn,Hz,DCM.Y.dt);
        ccf = ccf((size(ccf,1) + 1)/2,:,:);
        cc  = spm_cov2corr(squeeze(ccf));
        image(cc*64), title('Non-neuronal','FontSize',16), axis square

        subplot(2,4,4)
        ccf = spm_csd2ccf(Gy - Gn,Hz,DCM.Y.dt);
        ccf = ccf((size(ccf,1) + 1)/2,:,:);
        cc  = spm_cov2corr(squeeze(ccf));
        image(cc*64), title('Neuronal','FontSize',16), axis square

        % spectral decomposition
        %------------------------------------------------------------------
        subplot(2,2,3)
        plot(Hz,Hc),    hold on
        plot(Hz,Yc,':'),hold off
        title('Predicted and observed spectra','FontSize',16)
        axis square, spm_axis tight
        legend(name),legend('boxoff')

        subplot(2,2,4)
        plot(Hz,Hc,':'), hold on
        plot(Hz,Gc),     hold on
        plot(Hz,Nn,'-.'), hold off
        title('Neuronal and non-neuronal part','FontSize',16)
        axis square, spm_axis tight
        legend(name),legend('boxoff')

    %======================================================================
    % Kernels
    %======================================================================
    case lower('Kernels')

        % input effects
        %------------------------------------------------------------------
        x     = (1:DCM.M.N)*DCM.M.dt;
        for i = 1:ns

            % input effects - neural
            %--------------------------------------------------------------
            y = DCM.K1(:,:,i);
            subplot(ns,2,2*(i - 1) + 1)
            plot(x,y)
            set(gca,'XLim',[0 16])
            axis square
            title(['neural responses to ' name{i}])
            xlabel('time {seconds}')

            % input effects - BOLD
            %--------------------------------------------------------------
            y = DCM.K1(:,:,i);
            k = DCM.H1(:,:,i);
            subplot(ns,2,2*(i - 1) + 2)
            plot(x,y,':'), hold on
            plot(x,k)
            set(gca,'XLim',[0 16])
            axis square
            title('BOLD responses','FontSize',12)

        end
        legend(name),legend('boxoff')

    %======================================================================
    % Quit
    %======================================================================
    case lower('Quit')
        return;

    %======================================================================
    % Unknown action
    %======================================================================
    otherwise
        disp('Unknown option.')
end

% return to menu
%--------------------------------------------------------------------------
if nargin < 2
    spm_dcm_fmri_csd_results(DCM);
end

author: spm


text:
function gX = robOneDynamicsLatentGradients(model);

% ROBONEDYNAMICSLATENTGRADIENTS Gradients of the X vector given the dynamics model.

% FGPLVM

gX = zeros(size(model.X));

thetaDiff = model.theta(2:end)-model.theta(1:end-1);
while any(thetaDiff>pi)
  ind = find(thetaDiff>pi);
  thetaDiff(ind) = thetaDiff(ind) - 2*pi;
end
while any(thetaDiff<-pi)
  ind = find(thetaDiff<-pi);
  thetaDiff(ind) = thetaDiff(ind) + 2*pi;
end

logLikeTheta = log(2*pi*model.sigma2)...
    +(thetaDiff.*thetaDiff)/model.sigma2;
gLikeTheta = exp(-0.5*logLikeTheta)*model.mixTheta;
likeTheta = gLikeTheta + (1-model.mixTheta)/(2*pi);

thetaDiff = [0; thetaDiff; 0];
ratio = gLikeTheta./likeTheta;
ratio = [0; ratio; 0];
dL_dtheta = 1/model.sigma2*...
    (-thetaDiff(1:end-1).*ratio(1:end-1) ...
     +thetaDiff(2:end).*ratio(2:end));

logLikeR1 = model.a*log(model.b) - gammaln(model.a) ...
    +(model.a-1)*log(model.r) - model.b*model.r + log(model.mixR);
logLikeR1minus = (model.a-1)*log(model.b) - gammaln(model.a-1) ...
    +(model.a-2)*log(model.r) - model.b*model.r + log(model.mixR);
logLikeR2 = log(model.b) - model.b*model.r + log(1-model.mixR);

logLikeR1(find(logLikeR1<-316))=-316;
logLikeR1minus(find(logLikeR1minus<-316))=-316;
logLikeR2(find(logLikeR2<-316))=-316;

ind = find(logLikeR1>logLikeR2);
logLikeR = zeros(size(logLikeR1));
logLikeR(ind) = log(1+exp(logLikeR2(ind))./exp(logLikeR1(ind)))+logLikeR1(ind);

ind2 = find(logLikeR1<=logLikeR2);
logLikeR(ind2) = log(1+exp(logLikeR1(ind2))./exp(logLikeR2(ind2)))+logLikeR2(ind2);

%ll = ll + sum(logLikeR);

%logLikeR = model.a*log(model.b) - gammaln(model.a) ...
%    +(model.a-1)*log(model.r) - model.b*model.r;
%logLikeRminus = (model.a-1)*log(model.b) - gammaln(model.a-1) ...
%    +(model.a-2)*log(model.r) - model.b*model.r;
%likeR = exp(logLikeR)*model.mixR;
likeR = exp(logLikeR); %likeR = likeR + (1-model.mixR)*model.b*exp(-model.b*model.r);

dL_dr = (model.b*(exp(logLikeR1minus) - exp(logLikeR1)) ...
         - model.b*exp(logLikeR2))./likeR;

X1 = model.X(1:end-1, :);
X2 = model.X(2:end, :);
diffX = X2 -X1;
r2 = model.r.*model.r;
ind = find(model.r);
a = diffX(:, 1)./model.r;
a2 = a.*a;
oneMinusa2 = 1-a2;
if any(oneMinusa2==0)
  oneMinusa2 = 1-a2+eps;
end
dr_dxi2 = diffX(:, 2)./model.r;
dr_dxi1 = diffX(:, 1)./model.r;
dr_dximin12 = -diffX(:, 2)./model.r;
dr_dximin11 = -diffX(:, 1)./model.r;

dTheta_dxi1 = (1./model.r-diffX(:, 1).*dr_dxi1./r2)./sqrt(oneMinusa2);
dTheta_dxi2 = (-diffX(:, 1).*dr_dxi2./r2)./sqrt(oneMinusa2);
dTheta_dximin11 = (-1./model.r-diffX(:, 1).*dr_dximin11./r2)./sqrt(oneMinusa2);
dTheta_dximin12 = (-diffX(:, 1).*dr_dximin12./r2)./sqrt(oneMinusa2);

gX(2:end, 2) = dL_dtheta.*dTheta_dxi2 + dL_dr.*dr_dxi2;
gX(2:end, 1) = dL_dtheta.*dTheta_dxi1 + dL_dr.*dr_dxi1;

gX(1:end-1, 2) = gX(1:end-1, 2) + dL_dtheta.*dTheta_dximin12 + dL_dr.*dr_dximin12;
gX(1:end-1, 1) = gX(1:end-1, 1) + dL_dtheta.*dTheta_dximin11 + dL_dr.*dr_dximin11;

author: SheffieldML

repo: GPmat

save_path: github-repos/MATLAB/SheffieldML-GPmat


text:
%FAST_MEDIAN_IP In-place editing version of FAST_MEDIAN
%    MEDIANS = FAST_MEDIAN_IP(ARR)
%
% USE AT YOUR OWN RISK!
%
% See FAST_MEDIAN for general notes.
%
% This version edits the array in-place, so after you have run it your
% original array will have changed.  This is counter to standard Matlab
% style.  It also requires calling undocumented Mathworks internals, so is
% not guaranteed to work at all on all Matlab versions and should be used
% at your own risk.
%

% Version 0.87
% Peter H. Li 26-JUL-2013
% As required by MatLab Central FileExchange, licensed under the FreeBSD License


text:
% The COBRAToolbox: testEfmImport.m
%
% Purpose:
%     - test if EFMs can be imported from an external file
%
% Authors:
%     - Created initial test script: Chaitra Sarathy 2 Dec 19
%     - Updates to header docs: Chaitra Sarathy 19 Dec 19

global CBTDIR

% save the current path and initialize the test
currentDir = cd(fileparts(which(mfilename)));

% determine the test path for references
testPath = pwd;

% set the tolerance
% tol = 1e-8;

% load the model
model = readCbModel('testModel.mat','modelName','model_irr'); %For all models which are part of this particular test.

% Load reference data
load([testPath filesep 'testData_efmImport.mat']);

efmFileName = 'testEFMs.txt';
fluxFileName = 'testFluxes.txt';

% This is a function with two required and two optional inputs:
% Case 1: Test whether efmImport gives the expected output when using 2 input and 1 output argument
[EFMRxns_case1] = efmImport([testPath filesep], efmFileName);
assert(isequal(EFMRxns_case1,EFMRxns_ref));

% Case 2:  Test whether efmImport gives an error when using  2 input and 2 output arguments
% [EFMRxns_case2, EFMFluxes_case2] = efmImport([testPath filesep], efmFileName);
assert(verifyCobraFunctionError('efmImport', 'inputs', {[testPath filesep], efmFileName}, 'outputArgCount', 2));

% Case 3: Test whether efmImport gives the expected output when with all the 4 input and 2 output arguments
[EFMRxns_case4, EFMFluxes_case4] = efmImport([testPath filesep], efmFileName, [testPath filesep], fluxFileName);
assert(isequal(EFMRxns_case4, EFMRxns_ref));
assert(isequal(EFMFluxes_case4,EFMFluxes_ref));

author: opencobra

repo: cobratoolbox


text:
% Field value of each object in an array or cell array.
%
% See also: getfield

% Copyright 2010 Levente Hunyadi
function values = getclassfield(objects, field)

values = cell(size(objects));
if iscell(objects)
    for k = 1 : numel(values)
        values{k} = objects{k}.(field);
    end
else
    for k = 1 : numel(values)
        values{k} = objects(k).(field);
    end
end

author: CovertLab

repo: WholeCell

save_path: github-repos/MATLAB/CovertLab-WholeCell


text:
function [ paramAlpha, paramBeta ] = EstimateModelParameters( vT, vY )

vParamAlpha = [1, 2, 3];
vParamBeta  = [1, 2, 3];

bestMse = 1e50;

vX = zeros(size(vY, 1), 1, class(vY));

for ii = 1:length(vParamAlpha)
    currParamAlpha = vParamAlpha(ii);
    for jj = 1:length(vParamBeta)
        currParamBeta = vParamBeta(jj);
        vX(:) = currParamAlpha * (vT .^ currParamBeta);
        currMse = mean((vX - vY) .^ 2);
        if(currMse < bestMse)
            bestMse     = currMse;
            paramAlpha  = currParamAlpha;
            paramBeta   = currParamBeta;
        end
    end
end

end

author: RoyiAvital

repo: StackExchangeCodes


text:
function [varargout]=dicom3Dpar(D)

% function [v,OR,r,c]=dicom3Dpar(D)
% ------------------------------------------------------------------------
%
%
%
% ------------------------------------------------------------------------
%%

%Gets the 3D parameters from the DICOM info file
try
    v=D.PixelSpacing;
catch
    v=[1 1];
    warning('Missing field PixelSpacing, assuming [1 1]!');
end

%N.B. TO DO TREAT SPACING/THICKNESS PROPERLY (currently suitable for 3D
%volumes)
try
    v(3)=D.SpacingBetweenSlices;
catch
    try
        v(3)=D.SliceThickness;
    catch
        v(3)=1;
        warning('Missing field SpacingBetweenSlices and SliceThickness, assuming 1!');
    end
end

% ORIGIN:
% The x, y, and z coordinates of the upper left hand corner (center of the
% first voxel transmitted) of the image, in mm.
try
    OR=D.ImagePositionPatient;
catch
    OR=[0 0 0];
    warning('Missing field ImagePositionPatient, assuming [0 0 0]!');
end

try
    r=D.ImageOrientationPatient(4:6); % ROW DIRECTION
    c=D.ImageOrientationPatient(1:3); % COLUMN DIRECTION
catch
    r=[0 1 0];
    c=[1 0 0];
    warning('Missing field ImageOrientationPatient, assuming [1 0 0 0 1 0]!');
end

switch nargout
    case 1 %If only one output argument is requested then create structure
        G.v=v;
        G.OR=OR;
        G.r=r;
        G.c=c;
        varargout{1}=G;
    otherwise
        varargout{1}=v;
        varargout{2}=OR;
        varargout{3}=r;
        varargout{4}=c;
end

end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.

author: gibbonCode

repo: GIBBON

save_path: github-repos/MATLAB/gibbonCode-GIBBON


text:
function[varargout]=vzeros(varargin)
%VZEROS   Initializes multiple variables to arrays of zeros or nans.
%
%   [X1,X2, ... XN]=VZEROS(M,N)  is equivalent to
%
%      X1=ZEROS(M,N); X2=ZEROS(M,N); .... XN=ZEROS(M,N);
%
%   thus initializing all the output variables to arrays of zeros.
%
%   [X1,X2, ... XN]=VZEROS(M,N,NAN) initializes to NANs instead.
%   [X1,X2, ... XN]=VZEROS(M,N,INF) initializes to INFs instead.
%
%   [X1,X2, ... XN]=VZEROS(M,N,K,... P) similiarly initializes the Xi
%   to N-D arrays of zeros having size M x N x K x ... P.
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information (C)
%   2004--2010 J.M. Lilly --- type 'help jlab_license' for details

if nargin~=0
  if strcmpi(varargin{1}, '--t')
        vzeros_test,return
  end
end

bnan=0;
if ischar(varargin{end})
   str=lower(varargin{end});
   if strcmpi(str,'nan')
       bnan=1;
       varargin=varargin(1:end-1);
   elseif strcmpi(str,'inf')
       bnan=-1;
       varargin=varargin(1:end-1);
   end
else
   if isnan(varargin{end})
       bnan=1;
       varargin=varargin(1:end-1);
   elseif isinf(varargin{end})
       bnan=-1;
       varargin=varargin(1:end-1);
   end
end

for i=1:nargout
  if bnan==-1
    str=['varargout{' int2str(i) '}=inf*ones('];
  elseif bnan==1
    str=['varargout{' int2str(i) '}=nan*zeros('];
  else
    str=['varargout{' int2str(i) '}=zeros('];
  end

  for j=1:length(varargin)
      str=[str 'varargin{' int2str(j) '},'];
  end
  str=str(1:end-1);
  str=[str ');'];
  eval(str)
end

function[]=vzeros_test

z=zeros(5,4,2);
[x,y]=vzeros(5,4,2);
reporttest('VZEROS zeros case', all(x==z&y==z))
z=nan*zeros(5,4,2);
[x,y]=vzeros(5,4,2,nan);
vswap(x,y,z,nan,0);
reporttest('VZEROS nans case', all(x==z&y==z))
z=inf*ones(5,4,2);
[x,y]=vzeros(5,4,2,inf);
vswap(x,y,z,inf,0);

reporttest('VZEROS infs case', all(x==z&y==z))

author: jonathanlilly

repo: jLab

save_path: github-repos/MATLAB/jonathanlilly-jLab


text:
% eml_osem_example.m
%
% a complete example m-file illustrating E-ML-OSEM.
% showing faster "convergence" compared to E-ML-EM
%
% Copyright 1998-1, Jeff Fessler, University of Michigan

% generate data
if ~isvar('yi'), printm 'set up'
	f.count = 1e6;
	im clf, em_test_setup
	f.clim = [0 6];
prompt
end

% ML-EM iterations
if ~isvar('xmlem'), printm 'E-ML-EM'
	f.niter = 100;
	xinit = ig.ones; % uniform initial image

	im clf, im(xinit, f.clim, 'init')
	fun = @jf_show_iter;
	fun('mask', ig.mask, 'clim', f.clim, 'pause', 0); % initialize

	xmlem = eml_em(xinit(ig.mask), G, yi(:), ci(:), ri(:), ...
		'niter', f.niter, 'isave', 'all', 'userfun', fun);
	xmlem = ig.embed(xmlem);

	im plc 1 2
	im(1, xtrue, f.clim), cbar
	im(2, xmlem(:,:,end), 'ML-EM', f.clim), cbar
	printm('Done running ML-EM')
prompt
end

% block object for block iterations
if ~isvar('Gb'), printm 'Gb'
	f.nblock = 5;
	Gb = Gblock(G, f.nblock);
end

% OS-EM iterations
if ~isvar('xosem'), printm 'E-ML-OS-EM'
	im clf, im(xinit, f.clim, 'init')
	xosem = eml_osem(xinit(ig.mask), Gb, yi, ci, ri, ...
		'niter', f.niter, 'isave', 'all', 'precon', 'classic', ...
		'userfun', fun);
	xosem = ig.embed(xosem);

	im plc 1 3
	im(1, xtrue, f.clim), cbar
	im(2, xmlem(:,:,end), f.clim, 'ML-EM'), cbar
	im(3, xosem(:,:,end), f.clim, 'E-ML-OS-EM classic'), cbar
prompt
end

if 0, printm 'E-ML-OS-EM fast'
	xfast = eml_osem(xinit(ig.mask), Gb, yi, ci, ri, ...
		'niter', f.niter-1, 'precon', 'fast');
	xfast = ig.embed(xfast);

	im(3, xfast, 'E-ML-OS-EM fast'), cbar
	im(4, xfast-xosem, 'fast-classic'), cbar
prompt
end

% E-ML-INC-EM-3 iterations
if ~isvar('xiem3'), printm 'E-ML-INC-EM'

	xiem1 = eml_inc_em(xinit(ig.mask), Gb, yi, ci, ri, ...
		'niter', f.niter, 'hds', 1, 'isave', 'all');
	xiem1 = ig.embed(xiem1);

	xiem3 = eml_inc_em(xinit(ig.mask), Gb, yi, ci, ri, ...
		'niter', f.niter, 'hds', 3, 'isave', 'all');
	xiem3 = ig.embed(xiem3);

	im plc 1 2
	im(1, xiem1(:,:,end), f.clim, 'E-ML-INC-EM-1'), cbar
	im(2, xiem3(:,:,end), f.clim, 'E-ML-INC-EM-3'), cbar
	printm 'Done running E-ML-INC-EM'
prompt
end

if ~isvar('iplot') % plot likelihood to show acceleration
	iplot = 0:10;
	lfun = @(x) eql_obj(x(:,:,iplot+1), G, yi(:), ci(:), ri(:), [], ig.mask);
	like.mlem = lfun(xmlem);
	like.osem = lfun(xosem);
	like.iem1 = lfun(xiem1);
	like.iem3 = lfun(xiem3);
	if im
		im clf, plot(	...
			iplot, like.osem, 'c-+', ...
			iplot, like.iem1, 'g*-', ...
			iplot, like.iem3, 'm*-', ...
			iplot, like.mlem, 'yx-', ...
			f.nblock * iplot, like.osem, 'ro')
		axisx(0, max(iplot))
		legend('OS-EM', 'INC-EM-1', 'INC-EM-3', 'ML-EM', ...
			sprintf('OSEM/%d', f.nblock), 4)
		title 'ML-EM vs ML-OSEM convergence rate'
		xlabel iteration, ylabel likelihood
	end
prompt
end

% run regularized case for comparison
if ~isvar('R'), printm 'build R'
	f.l2b = -2;
	f.delta = 0.5;
	R = Reg1(ig.mask, 'type_denom', 'matlab', ...
		'pot_arg', {'hyper3', f.delta}, 'beta', 2^f.l2b);
%prompt
end

if ~isvar('xh'), printm 'E-PL-OS-EMDP'
	f.niter_pl = 31;
	im clf
	xh = epl_os_emdp(xinit(ig.mask), Gb, yi, ci, ri, R, ...
		'niter', f.niter_pl, 'userfun', fun, 'isave', 'last');
	xh = ig.embed(xh);
	im plc 1 3
	im(1, xtrue, f.clim), cbar
	im(2, xmlem(:,:,end), f.clim), cbar
	im(3, xh, 'PL-OS-EMDP', f.clim), cbar
%prompt
end

if ~isvar('elim') % compare pics of ml-osem vs pl
	elim = [-1 1]*2.5;
	im plc 2 3
	im(1, xtrue, f.clim, 'true'), cbar
	im(2, xosem(:,:,end), f.clim, 'ML-OSEM'), cbar
	im(3, xh(:,:,end), f.clim, 'PL-OS-EMDP'), cbar
	im(5, xtrue-xosem(:,:,end), elim), cbar
	titlef('nrms err %g%%', 100*nrms(col(xosem(:,:,end)), xtrue(:)))
	im(6, xtrue-xh(:,:,end), elim), cbar
	titlef('nrms err %g%%', 100*nrms(col(xh(:,:,end)), xtrue(:)))
prompt
end

if 1
	im clf
	im([xtrue; xmlem(:,:,end); xh(:,:,end)], f.clim, 'True | ML | PL')
prompt
end

if ~isvar('xcake') % illustrate space-variant PSF
	[px py] = ndgrid(3:7:ig.nx, 3:7:ig.ny);
	xcake = ig.zeros;
	xcake(px(:), py(:)) = 1;
	xcake = xcake .* ig.mask;
	im(xcake)

	if ~isvar('xh2'), printm 'E-PL-OS-EMDP'
		im clf
		frac = 0.4;
		ytmp = yi + frac * ci .* (G * xcake);
		xh2 = epl_os_emdp(xinit(ig.mask), Gb, ytmp, ci, ri, R, ...
			'niter', f.niter_pl, 'userfun', fun, 'isave', 'last');
		xh2 = ig.embed(xh2);
	end
%prompt
end
	im plc 2 2
	im(1, xh, 'PL Original', f.clim), cbar
	im(2, xcake), cbar
	im(3, xh2, 'PL with point perturbations', f.clim), cbar
	im(4, (xh2 - xh) / frac, 'PSF', [-0.1 0.5]), cbar

author: JeffFessler

repo: mirt

save_path: github-repos/MATLAB/JeffFessler-mirt


text:
function [P_FIX,P_LO,P_BS,P_UE,P_COD,P_DEC,L_BS,P_BT] = functionCPmodel(valueset)
%This function return the values of the CP model coefficients in Table 5.3.
%
%INPUT:
%valueset = Select which one of the value sets in Table 5.3 that is
%           considered. Either valueset=1 or valueset=2 are possible.
%
%OUTPUT:
% P_FIX = Fixed circuit power per BS (in Watt)
% P_LO  = Circuit power per LO (in Watt)
% P_BS  = Circuit power per BS antenna (in Watt)
% P_UE  = Circuit power UE (in Watt)
% P_COD = Circuit power for encoding
% P_DEC = Circuit power for decoding
% L_BS  = BS computational efficiency
% P_BT  = Circuit power for backhaul
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%% Define parameter values for Value set 1
if valueset == 1

    P_FIX = 10;

    P_LO = 0.2;

    P_BS = 0.4;

    P_UE = 0.2;

    P_COD = 0.1*10^(-9);

    P_DEC = 0.8*10^(-9);

    L_BS = 75*10^9;

    P_BT = 0.25*10^(-9);

% Define parameter values for Value set 2
elseif valueset == 2

    P_FIX = 5;

    P_LO = 0.1;

    P_BS = 0.2;

    P_UE = 0.1;

    P_COD = 0.01*10^(-9);

    P_DEC = 0.08*10^(-9);

    L_BS = 750*10^9;

    P_BT = 0.025*10^(-9);

end

author: emilbjornson

repo: massivemimobook


text:
function [iscellinfo] = bz_isCellInfo(cellinfo)
% USAGE
% [] = bz_isCellInfo(spikes)
%
% INPUT
%       cellinfo  - struct with the following fields
%                   .UID
%                   .sessionName
%                   .region
%
% OUTPUT
%      logical true if struct meets cellinfo criteria, false if otherwise
%
% written by david tingley, 2017

if isfield(cellinfo,'UID') && isfield(cellinfo,'sessionName') && isfield(cellinfo,'region') % check that fields exist
     if ischar(cellinfo.sessionName) && isvector(cellinfo.UID) && isvector(cellinfo.region)
         iscellinfo = true;
     else
         warning('one of the required fields for a cellinfo type is not formatted correctly')
         iscellinfo = false;
     end
else
    warning('one of the required fields for a cellinfo type does not exist')
    iscellinfo = false;
end

author: buzsakilab

repo: buzcode

save_path: github-repos/MATLAB/buzsakilab-buzcode


text:
function determ = gk323_determinant ( n )

%*****************************************************************************80
%
%% GK323_DETERMINANT returns the determinant of GK323.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%   09 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  determ = ( -1 )^( n - 1 ) * 2^( n - 2 ) * ( n - 1 );

  return
end

author: johannesgerer

repo: jburkardt-m


text:
 function [pmf xs ys] = kde_pmf2(x, y, varargin)
%function [pmf xs ys] = kde_pmf2(x, y, varargin)
%|
%| Compute a 2D empirical joint probability mass function (PMF)
%| using interpolation that is similar to a kernel density estimate.
%| This joint PMF (interpolated / smoothed histogram) is useful for
%| computing joint entropy and other similarity metrics for image registration.
%|
%| in
%|	x	[N 1]
%|	y	[N 1]	iid realizations of random variable pairs (x,y)
%|
%| option
%|	'dx'		bin spacing (default: 1)
%|			use 'silverman' to choose dx based on rule-of-thumb
%|			using inter-quartile range (p48 of 1986 book)
%|	'dy'		bin spacing (default: dx)
%|	'loop'		method; default is to try 'mex' if available, else 'pmf'
%|	'chat'	0|1	print out intermediate values? (default: 0)
%|
%| out
%|	pmf	[Kx Ky]	nonnegative and sum to 1
%|	xs	[Kx 1]	sample locations, differing by dx
%|	ys	[Ky 1]	sample locations, differing by dy
%|
%| Code assumes kernel support is (-width/2, width/2).
%| Default is a quadratic spline supported on (-3/2,3/2).
%|
%| Copyright 2010-07-31, Jeff Fessler, University of Michigan

if nargin == 1 && streq(x, 'test'), kde_pmf2_test, return, end
if nargin < 2, ir_usage, end

arg.dx = 1;
arg.dy = []; % dx below
arg.kernel = @kde_pmf2_bspline2;
arg.widthx = 3; % kernel width, must match kernel!
arg.widthy = []; % widthx below
arg.loop = ''; % see below
arg.chat = 0;
arg = vararg_pair(arg, varargin);

if isempty(arg.loop)
	if has_mex_jf
		arg.loop = 'mex';
	else
		arg.loop = 'pmf';
	end
end
if isempty(arg.dy)
	arg.dy = arg.dx;
end
if isempty(arg.widthy)
	arg.widthy = arg.widthx;
end

x = x(:);
y = y(:);

% eqn (3.31) on p 48 of Silverman 1986, which is for gaussian.
% here converted to match the FWHM for quadratic bspline
if streq(arg.dx, 'silverman')
	arg.dx = kde_pmf_width(x, 'chat', arg.chat);
end
if streq(arg.dy, 'silverman')
	arg.dy = kde_pmf_width(y, 'chat', arg.chat);
end

if streq(arg.loop, 'mex')
	if ~isequal(arg.kernel, @kde_pmf2_bspline2) ...
		|| arg.widthx ~= 3 || arg.widthy ~= 3
		fail 'mex only supports bspline2 with width=3'
	end
	[pmf xs ys] = jf_mex('kde,b2', single([x y]), single([arg.dx arg.dy]));

else
	[pmf xs ys] = kde_pmf2_do(x, y, ...
		arg.dx, arg.dy, arg.widthx, arg.widthy, ...
		arg.kernel, arg.kernel, arg.loop, arg.chat);
end

if ~nargout && im
	im plc 1 2
	[his cen] = jf_histn([x(:) y(:)]);
	im(1, cen{1}, cen{2}, his, 'histogram')
	im(2, xs, ys, pmf, 'pmf')
	clear pmf
end

% kde_pmf2_do()
function [pmf xs ys] = kde_pmf2_do(x, y, dx, dy, wx, wy, kernx, kerny, ...
	loop, chat)

if kernx(-wx/2) ~= 0 || kernx(wx/2) ~= 0
	fail 'kernel must be zero at +/- width/2 (and beyond)'
end
if kerny(-wy/2) ~= 0 || kerny(wy/2) ~= 0
	fail 'kernel must be zero at +/- width/2 (and beyond)'
end

N = numel(x);
if N ~= numel(y), fail('x and y must be same size'), end

kmin = floor(min(x)/dx - wx/2) + 1;
kmax = ceil(max(x)/dx + wx/2) - 1; % kmin <= k <= kmax
Kx = kmax - kmin + 1;
xs = (kmin:kmax)' * dx;
x = x - dx * (kmin-1); % wlog, shift so that kmin=1 (for matlab)
%if chat, pr [kmin kmax], end

kmin = floor(min(y)/dy - wy/2) + 1;
kmax = ceil(max(y)/dy + wy/2) - 1; % kmin <= k <= kmax
Ky = kmax - kmin + 1;
ys = (kmin:kmax)' * dy;
y = y - dy * (kmin-1); % wlog, shift so that kmin=1 (for matlab)
%if chat, pr [kmin kmax], end
clear kmin kmax

switch loop

case 'none' % no loop, but O(NK) and huge memory use!
	kx = 1:Kx;
	ky = 1:Ky;
	tmp1 = kernx(outer_sum(x/dx, -kx)); % [N Kx]
	tmp2 = kerny(outer_sum(y/dy, -ky)); % [N Ky]
	pmf = tmp1' * tmp2; % [Kx Ky]

case 'data' % loop over data N
	pmf = zeros(Kx,Ky);
	for nn=1:N
		xn = x(nn);
		yn = y(nn);
		k1 = floor(xn/dx - wx/2) + 1;
		k2 = ceil(xn/dx + wx/2) - 1;
		kx = (k1:k2)';
		k1 = floor(yn/dy - wy/2) + 1;
		k2 = ceil(yn/dy + wy/2) - 1;
		ky = (k1:k2);
		pmf(kx,ky) = pmf(kx,ky) + ...
			kernx(xn/dx - kx) * kerny(yn/dy - ky); % outer prod
	end

case 'pmf' % loop over PMF bins K
	pmf = zeros(Kx,1);
	for ky=1:Ky
	 for kx=1:Kx
		xmin = ((kx - wx/2)) * dx;
		xmax = ((kx + wx/2)) * dx;
		good = xmin < x & x < xmax;
		ymin = ((ky - wy/2)) * dy;
		ymax = ((ky + wy/2)) * dy;
		good = (ymin < y & y < ymax) & good;
		tmp = kernx(x(good)/dx - kx) .* kerny(y(good)/dy - ky);
		pmf(kx,ky) = sum(tmp);
	 end
	end

otherwise
	fail('unknown loop type "%s"', loop)
end
pmf = pmf / N;

% quadratic B-spline
% kde_pmf2_bspline2()
function y = kde_pmf2_bspline2(x)
y = (3/4 - x.^2) .* (abs(x) < 1/2) + ...
	(abs(x) - 3/2).^2 / 2 .* (1/2 <= abs(x) & abs(x) < 1.5);

% test routine
function kde_pmf2_test

rng(0)
n = 500;
sig = 5;
rho = 0.9; % correlated gaussian
Cov = sig^2 * [1 rho; rho 1];

%h1 = (1 + log(2*pi*sig^2)) / 2; % differential entropy for 1D gaussian
h = (size(Cov,1) + log(det(2*pi*Cov))) / 2; % differential entropy for gaussian
pr h

tmp = randn(n, 2) * sqrtm(Cov);
x = tmp(:,1);
y = tmp(:,2);

if 0 % uniform
	x = 10 * (rand(n,1) - 0.5);
	y = 10 * (rand(n,1) - 0.5);
end

if 1 % check h
	dx = 2.4;
	dy = 2.8;
	[pmf xs ys] = kde_pmf2(x, y, 'dx', dx, 'dy', dy);
	equivs(sum(pmf(:)), 1)
	Hf = @(p) sum(-p(p>0) .* log(p(p>0)));
	H = Hf(pmf);
	pr H
	pr H + log(dx*dy)
end

args = {x, y, 'dx', 'silverman'};
[pmf xs ys] = kde_pmf2(args{:}, 'chat', 1);

if 1 % check other loop methods
	loop_list = {'none', 'pmf', 'data'};
	if has_mex_jf
		loop_list{end+1} = 'mex';
	end
	for ii=1:numel(loop_list)
		cpu etic
		[pmfn xsn ysn] = kde_pmf2(args{:}, 'loop', loop_list{ii});
		equivs(pmf, pmfn, 'thresh', 2e-6)
		equivs(xs, xsn)
		equivs(ys, ysn)
		cpu('etoc', sprintf('%s: %4s', mfilename, loop_list{ii}))
	end
end

if 0 && has_mex_jf
	[pmfm xsm ysm] = kde_pmf2(args{:}, 'loop', 'mex');
%	im(xsm, ysm, pmfm)
	equivs(xsm, xs)
	equivs(ysm, ys)
	equivs(pmf, pmfm)
end

if im
	im plc 1 3
	[hist cent] = jf_histn([x y], 'nbin', 30);
	im(1, cent{1}, cent{2}, hist/n)
	im(2, xs, ys, pmf)

	dx = linspace(0.5, 3.5, 11);
	nd = numel(dx);
	for id=1:nd
		tmp = kde_pmf2(x, y, 'dx', dx(id));
		Hv(id) = Hf(tmp); % H for several dx values
	end

	im subplot 3
	plot(dx([1 end]), [h h], '-', ...
		dx, Hv, 'o', ...
		dx, Hv + log(dx.*dx), '+') % H + log(dx*dy) approximately is h
	legend('h', 'H', 'H + log(dx dy)', 'location', 'southwest'), grid
end

if 0
	clf
	while(1)
		im(cent{:}, hist/n)
		axis([-1 1 -1 1]*10)
		prompt
		im(xs, ys, pmf)
		axis([-1 1 -1 1]*10)
		prompt
	end
end


text:
% GoHalfSitting.m
%%%%%%%%%%% Move to non singular pose %%%%%%%%%%%%
uLINK(RLEG_J2).q = -5.0*ToRad;
uLINK(RLEG_J3).q = 10.0*ToRad;
uLINK(RLEG_J4).q = -5.0*ToRad;

uLINK(LLEG_J2).q = -5.0*ToRad;
uLINK(LLEG_J3).q = 10.0*ToRad;
uLINK(LLEG_J4).q = -5.0*ToRad;

uLINK(BODY).p = [0.0, 0.0, 0.7]';
uLINK(BODY).R = eye(3);

author: s-kajita

repo: IntroductionToHumanoidRobotics


text:
function geometry_test014 ( )

%*****************************************************************************80
%
%% TEST014 tests CIRCLE_EXP_CONTAINS_POINT_2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  dim_num = 2;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST014\n' );
  fprintf ( 1, '  CIRCLE_EXP_CONTAINS_POINT_2D determines if a\n' );
  fprintf ( 1, '    point lies inside a circle.\n' );
%
%  This point is inside.
%
  p1(1:dim_num) = [  4.0, 2.0 ];
  p2(1:dim_num) = [  1.0, 5.0 ];
  p3(1:dim_num) = [ -2.0, 2.0 ];
  p(1:dim_num)  = [  2.0, 3.0 ];

  r8vec_print ( dim_num, p1, '  P1:' );
  r8vec_print ( dim_num, p2, '  P2:' );
  r8vec_print ( dim_num, p3, '  P3:' );
  r8vec_print ( dim_num, p, '  P:' );

  inside = circle_exp_contains_point_2d ( p1, p2, p3, p );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  INSIDE = %d\n', inside );
  if ( inside == -1 )
    fprintf ( 1, '  The point is inside the circle.\n' );
  elseif ( inside == 0 )
    fprintf ( 1, '  The point is on the circle.\n' );
  elseif ( inside == 1 )
    fprintf ( 1, '  The point is outside the circle\n' );
  elseif ( inside == 2 )
    fprintf ( 1, '  Colinear data, the point is on the line.\n' );
  elseif ( inside == 3 )
    fprintf ( 1, '  Colinear data, the point is not on the line.\n' );
  elseif ( inside == 4 )
    fprintf ( 1, '  Two equal data points, the point is on the line.\n' );
  elseif ( inside == 5 )
    fprintf ( 1, '  Two equal data points, the point is not on the line.\n' );
  elseif ( inside == 6 )
    fprintf ( 1, '  All data points equal, the point is equal.\n' );
  elseif ( inside == 7 )
    fprintf ( 1, '  All data points equal, the point is not equal.\n' );
  end
%
%  This point is actually right on the circle.
%
  p1(1:dim_num) = [  4.0,  2.0 ];
  p2(1:dim_num) = [  1.0,  5.0 ];
  p3(1:dim_num) = [ -2.0,  2.0 ];
  p(1:dim_num)  = [  1.0, -1.0 ];

  r8vec_print ( dim_num, p1, '  P1:' );
  r8vec_print ( dim_num, p2, '  P2:' );
  r8vec_print ( dim_num, p3, '  P3:' );
  r8vec_print ( dim_num, p, '  P:' );

  inside = circle_exp_contains_point_2d ( p1, p2, p3, p );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  INSIDE = %d\n', inside );
  if ( inside == -1 )
    fprintf ( 1, '  The point is inside the circle.\n' );
  elseif ( inside == 0 )
    fprintf ( 1, '  The point is on the circle.\n' );
  elseif ( inside == 1 )
    fprintf ( 1, '  The point is outside the circle\n' );
  elseif ( inside == 2 )
    fprintf ( 1, '  Colinear data, the point is on the line.\n' );
  elseif ( inside == 3 )
    fprintf ( 1, '  Colinear data, the point is not on the line.\n' );
  elseif ( inside == 4 )
    fprintf ( 1, '  Two equal data points, the point is on the line.\n' );
  elseif ( inside == 5 )
    fprintf ( 1, '  Two equal data points, the point is not on the line.\n' );
  elseif ( inside == 6 )
    fprintf ( 1, '  All data points equal, the point is equal.\n' );
  elseif ( inside == 7 )
    fprintf ( 1, '  All data points equal, the point is not equal.\n' );
  end
%
%  This point is outside.
%
  p1(1:dim_num) = [  4.0, 2.0 ];
  p2(1:dim_num) = [  1.0, 5.0 ];
  p3(1:dim_num) = [ -2.0, 2.0 ];
  p(1:dim_num)  = [  4.0, 6.0 ];

  r8vec_print ( dim_num, p1, '  P1:' );
  r8vec_print ( dim_num, p2, '  P2:' );
  r8vec_print ( dim_num, p3, '  P3:' );
  r8vec_print ( dim_num, p, '  P:' );

  inside = circle_exp_contains_point_2d ( p1, p2, p3, p );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  INSIDE = %d\n', inside );
  if ( inside == -1 )
    fprintf ( 1, '  The point is inside the circle.\n' );
  elseif ( inside == 0 )
    fprintf ( 1, '  The point is on the circle.\n' );
  elseif ( inside == 1 )
    fprintf ( 1, '  The point is outside the circle\n' );
  elseif ( inside == 2 )
    fprintf ( 1, '  Colinear data, the point is on the line.\n' );
  elseif ( inside == 3 )
    fprintf ( 1, '  Colinear data, the point is not on the line.\n' );
  elseif ( inside == 4 )
    fprintf ( 1, '  Two equal data points, the point is on the line.\n' );
  elseif ( inside == 5 )
    fprintf ( 1, '  Two equal data points, the point is not on the line.\n' );
  elseif ( inside == 6 )
    fprintf ( 1, '  All data points equal, the point is equal.\n' );
  elseif ( inside == 7 )
    fprintf ( 1, '  All data points equal, the point is not equal.\n' );
  end

  return
end


text:
function spline_test21 ( )

%*****************************************************************************80
%
%% TEST21 tests SPLINE_LINEAR_VAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author
%
%    John Burkardt
%
  n = 11;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST21\n' );
  fprintf ( 1, '  SPLINE_LINEAR_VAL evaluates a linear spline.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Runge''s function, evenly spaced knots.\n' );

  t = linspace ( -1.0E+00, 1.0E+00, n )';
  y = frunge ( t );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'T, Y\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( '%14f  %14f\n', t(i), y(i) );
  end

  for i = 0 : n

    if ( i == 0 )
      jhi = 1;
    elseif ( i < n )
      jhi = 2;
    else
      jhi = 2;
    end

    for j = 1 : jhi

      if ( i == 0 )
        tval = t(1) - 1.0E+00;
      elseif ( i < n )
        tval = ( ( jhi + 1 - j ) * t(i) + ( j - 1 ) * t(i+1) ) / jhi;
      else
        if ( j == 1 )
          tval = t(n);
        else
          tval = t(n) + 1.0E+00;
        end
      end

      [ yval, ypval ] = spline_linear_val ( n, t, y, tval );

      fval = frunge ( tval );

      fprintf ( 1, '\n' );
      fprintf ( 1, '%12f  %12f  %12f\n', tval, yval, fval );

    end

  end

  return
end


text:
function c=v_lpccc2cc(cc,np)
%V_LPCCC2PF Extrapolate complex cepstrum C=(CC)

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: v_lpccc2cc.m 10865 2018-09-21 17:22:45Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

p=size(cc,2);
if nargin<2 np=p; end
if np<=p
   c=cc(:,1:np);
else
   c=v_lpcar2cc(v_lpccc2ar(cc),np);
end

author: ImperialCollegeLondon

repo: sap-voicebox


text:
function [C,S,a,c,CStdDev,SStdDev]=getEGMGravCoeffs(M,isTideFree,modelType,TT1,TT2,adjType,useIERSC20)
%%GETEGMGRAVCOEFFS Get fully normalized spherical harmonic coefficients for
%                  the National Geospatial Intelligence Agency's (NGA's)
%                  Earth Gravitation Model 2008 (EGM2008) or Earth
%                  Gravitation Model 1996 (EGM96) models up to a desired
%                  order. The coefficients can be for the zero-tide or the
%                  tide-free models. Note that time-varying components
%                  except for the drift of the coefficients over time and
%                  the pole tide (if requested) are not included and must
%                  be added separately. The coefficients can be used with
%                  the spherHarmonicEval function to provide an
%                  acceleration with respect to the WGS-84 coordinate
%                  system (the ITRS) but WITHOUT the Coriolis term or any
%                  other terms that would arise due to the fact that the
%                  coordinate system is non-inertial.
%
%INPUTS: M The integer maximum order of the spherical harmonic coefficients
%          obtained. This is a value between 2 and 2190 for the EGM2008
%          model and between 2 and 360 for the EGM96 model. If this
%          parameter is omitted or an empty matrix is passed, the default
%          value being the total number of coefficients in the model is
%          used.
% isTideFree A boolean value indicating whether the coefficients should be
%          for a theoretical tide-free Earth or for a zero-tide model. If
%          this parameter is omitted, the default value is isTideFree=true;
% modelType An optional parameter specifying the coefficient model to load.
%          Possible values are
%          0 (the default if omitted) Load the EGM2008 model.
%          1 Load the EGM96 model.
%  TT1,TT2 An optional input. Two parts of a Julian date given in
%          terrestrial time (TT). The units of the date are days. The full
%          date is the sum of both terms. The date is broken into two parts
%          to provide more bits of precision. It does not matter how the
%          date is split. The use of the date depends on the subsequent
%          parameter adjType.
%  adjType An optional parameter specifying what adjustments (if any)
%          should be made to the coefficients. Possible values are
%          0) (The default if omitted) Do not make any adjustments. The TT
%             values (if provided) will not be used.
%          1) Adjust for the drift of the low-order coefficients over time
%             as given by the respective models. The drifts over time occur
%             for many reasons, such as because of the rebound of the
%             tectonic plates after the glaciers receded at the end of the
%             last ice age. The TT values are required. The
%             getGravCoeffOffset4Drift function is used.
%          2) Adjust for polar motion. Polar motion is a change in the
%             rotation axis of the Earth with respect to the crust and
%             according to the 2010 IERS conventions, it affects the lower-
%             order spherical harmonic terms. The
%             getAdjustedGravCoeffs4PolarMotion function is used after the
%             drift of the low-order coefficients is taken into account,
%             but before the tide-free model is converted to a zero-tide
%             model (if desired). The inputs to the function deltaTTUT1 and
%             clockLoc are set to default values.
%          3) Adjust for the drift of the coefficients and for polar
%             motion.
% useIERSC20 This input is only used if the EGM2008 model is chosen. The
%          IERS 2010 conventions provides the drift rates for the
%          gravitational terms, but defines a different more accurate value
%          of C_{2,0}. If this is true, then the more accurate value is
%          used. If this is false, then the value in the EGM2008 model is
%          used. If omitted, the default is false if adjType=0 and true for
%          any other value of adjType.
%
%OUTPUTS: C An array holding the coefficient terms that are multiplied by
%           cosines in the harmonic expansion. This can be given to a
%           CountingClusterSet class so that C(n+1,m+1) is the coefficient
%           of degree n and order m. When a maximum degree of M is used,
%           all C have values for all n from 0 to M and for all m from 0 to
%           n for each n. The coefficients are unitless.
%         S An array holding the coefficient terms that are multiplied by
%           sines in the harmonic expansion. The format of S is the same as
%           that of C.
%         a The numerator in the (a/r)^n term in the spherical harmonic
%           sum, having units of meters.
%         c The constant value by which the spherical harmonic series is
%           multiplied, having units of m^3/s^2.
%   CStdDev An array holding the standard deviation of each C coefficient.
%   SStdDev An array holding the standard deviation of each S coefficient.
%
%The EGM2008 coefficients are available from the NGA on the site:
%http://earth-info.nga.mil/GandG/wgs84/gravitymod/egm2008/egm08_wgs84.html
%where one should download the "EGM2008 Tide Free Spherical Harmonic
%Coefficients", which is the file EGM2008_to2190_TideFree.gz. The text file
%should be ungzipped and placed in the data folder that is in the same
%folder as this script. If no external program is available to unzip the
%file, the Matlab function gunzip can be used.
%
%The EGM96 coefficients are available from the NGA on the site:
%http://earth-info.nga.mil/GandG/wgs84/gravitymod/egm96/egm96.html
%The coefficients should be decompressed (resulting in a text file named
%egm96 without a file extension). The decompressed file should be placed in
%the data folder that is in the same folder as this script.
%
%This function first checks for a .mat file with the coefficients in it.
%The .mat file is small and can be read quite quickly. However, if one does
%not exist, then it tries to read the EGM2008_to2190_TideFree text file (or
%the egm96 text file) that one can obtain directly from the NGA. If
%M is the maximum number of coefficients or is empty and the text file is
%read directly, then the .mat file is created so that subsequent reads are
%faster. Note that after the .mat file has ben created, the text file can
%be deleted.
%
%More on using the spherical harmonic coefficients is given in
%the comments for the function spherHarmonicEval and the format and use of
%the coefficients is also documented in
%http://earth-info.nga.mil/GandG/wgs84/gravitymod/egm2008/README_FIRST.pdf
%
%The EGM2008 model is documented in [1] and [2]. The EGM96 model is
%documented in [3].
%
%The IERS conventions are in [4].
%
%REFERENCES:
%[1] N. K. Pavlis, S. A. Holmes, S. C. Kenyon, and F. J. K., "The
%    development and evaluation of the Earth gravitational model 2008
%    (EGM2008)," Journal of Geophysical Research, vol. 117, no. B4, Apr.
%    2012.
%[2] N. K. Pavlis, S. A. Holmes, S. C. Kenyon, and F. J. K., "Correction to
%    "The development and evaluation of the Earth gravitational model 2008
%    (EGM2008)"," Journal of Geophysical Research, vol. 118, 2013.
%[3] F. G. Lemoine, S. Kenyon, J. Factor, R. G. Trimmer, N. K. Pavlis, and
%    et. al., "The development of the joint NASA GSFC and the National
%    Imagery and Mapping Agency (NIMA) geopotential model EGM96," National
%    Aeronautics and Space Administration, Goddard Space Flight Center,
%    Greenbelt, MD, Tech. Rep. NASA/TP-1998-206861, Jul. 1998.
%[4] G. Petit and B. Luzum, IERS Conventions (2010), International Earth
%    Rotation and Reference Systems Service Std. 36, 2010.
%
%March 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<6)
    adjType=0;
end

if(nargin<7)
    if(adjType==0)
       useIERSC20=false;
    else
       useIERSC20=true;
    end
end

if(nargin<5)
    TT1=[];
    TT2=[];
end

if(nargin<3)
    modelType=0;%EGM2008 Model
end

if(nargin<2)
    isTideFree=true;
end

if(nargin<1||isempty(M))
    if(modelType==0)
        M=2190;%EGM2008 Model
    else
        M=360;%EGM96 Model
    end
end

if(modelType==0&&M>2190)
    error('The EGM2008 model only has coefficients up through degree 2190');
elseif(modelType==1&&M>360)
    error('The EGM96 model only has coefficients up through degree 360');
end

%The same values of a and c are used in the EGM2008 mnodel as in the EGM96
%model.
a=Constants.EGM2008SemiMajorAxis;
c=Constants.EGM2008GM;

if(modelType==0)
    fileName='/data/EGM2008_to2190_TideFree';
else
    fileName='/data/egm96';
end

totalNumCoeffs=(M+1)*(M+2)/2;

%The EGM20008 and EGM96 coefficient data files should be located in a data
%folder that is in the same folder as this file. This finds the path to
%this file.
ScriptPath=mfilename('fullpath');
ScriptFolder=fileparts(ScriptPath);

%First, see if a .mat file with all of the data exists. If so, then use
%that and ignore everything else.
if(exist([ScriptFolder,fileName,'.mat'],'file'))
    load([ScriptFolder,fileName,'.mat'],'C','S','CStdDev','SStdDev');

    C=C(1:totalNumCoeffs);
    S=S(1:totalNumCoeffs);
    CStdDev=CStdDev(1:totalNumCoeffs);
    SStdDev=SStdDev(1:totalNumCoeffs);
else
    %If the .mat file does not exist, then assume that the coefficients
    %must be read from the text file provided by the NGA and, if all of the
    %coefficients were read in, then create the .mat file after reading in
    %the coefficients so that future reads are faster

    %Allocate space for the coefficients.
    emptyData=zeros(totalNumCoeffs,1);
    C=emptyData;
    S=emptyData;
    CStdDev=emptyData;
    SStdDev=emptyData;

    %Read in the data up to the specified order. The -3 deals with the fact
    %that coefficients for degrees and order (0,0), (1,0) and (1,1) have
    %been omitted from the data file.
    fileID=fopen([ScriptFolder,fileName]);
    data=textscan(fileID,'%d %d %f %f %f %f',totalNumCoeffs-3);
    fclose(fileID);

    %The data is already appropriately arranged by degree and order
    %starting with degree 2 and order 0.
    C(4:end)=data{3};
    S(4:end)=data{4};
    CStdDev(4:end)=data{5};
    SStdDev(4:end)=data{6};

    %The EGM96 and EGM2008 coefficient files omits the C_{0,0} value. It is
    %explicitly added here.
    C(1)=1;

    %If all of the coefficients have been read, then save the data as a
    %.mat file so that it can be read faster in the future.
    if(modelType==0&&M==2190||modelType==1&&M==360)
        save([ScriptFolder,fileName,'.mat'],'C','S','CStdDev','SStdDev');
    end
end

%If the value for the EGM2008 C20 term from the IERS2008 model should be
%used. The C20 terms are kept separately, because it is implied in the IERS
%conventions that
if(modelType==0&&useIERSC20==true)
    %The zero-tide value is from Table 6.2 in the 2010 IERS conventions.
    C20ZeroTide=-0.48416948e-3;
    C20TideFree=-0.48416531e-3;
elseif(modelType==0)
    C20TideFree=C(4);
    %This value is documented on the web site providing the EGM2008
    %coefficients.
    C20ZeroTide=-0.484169317366974e-3;
else%The values for the EGM96 model
    C20TideFree=C(4);
    %The formula is in the readme file accompanying the EGM96 model.
    C20ZeroTide=C(4)-3.11080e-8*0.3/sqrt(5);
end

%If the coefficients should be updated for the drift over time.
if(~isempty(TT1)&&adjType~=0&&adjType~=2)
    %Get the offsets due to drift over time.
    [deltaC,deltaS]=getGravCoeffOffset4Drift(TT1,TT2,modelType);

    %Add in the offsets
    numOffset=length(deltaC);
    if(numOffset<=totalNumCoeffs)
        C(1:numOffset)=C(1:numOffset)+deltaC(:);
        S(1:numOffset)=S(1:numOffset)+deltaS(:);
    else
        C(:)=C(:)+deltaC(1:numEl);
        S(:)=S(:)+deltaS(1:numEl);
    end

    C20TideFree=C20TideFree+deltaC(4);
    C20ZeroTide=C20ZeroTide+deltaC(4);
end

%If the coefficients should be updated for the effects of polar motion.
if(adjType==2||adjType==3)
    %The IERS conventions implies that the zero-tide value of C20 should be
    %used in this computation
    C(4)=C20ZeroTide;
    [~,~,C,S]=getAdjustedGravCoeffs4PolarMotion(C,S,TT1,TT2,true);
end

%Set the C20 value for the appropriate model.
if(isTideFree==false)
    C(4)=C20ZeroTide;
else
    C(4)=C20TideFree;
end

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.

author: USNavalResearchLaboratory

repo: TrackerComponentLibrary


text:
function [num,den] = termextr(term_mat,T)
%
% Utility function: TERMEXTR
%
% The purpose of this function is to extract the terms out of the
% term matrix created within the controls tutor

% Author: Craig Borghesani
% Date: 8/17/94
% Revised: 10/27/94
% Copyright (c) 1999, Prentice-Hall

if all(size(term_mat)==[1,4]), % pid controller
 if term_mat(4) == 40, term_mat = pid2pid(term_mat,1); end
 term_mat = termpars(term_mat([3,1,2]),[1,0]);
end

% Convert Lead/Lag terms into pole and zero terms
term_mat = termcnvt(term_mat);

num=1;nc=0;den=1;dc=0;
if nargin==1,
 for p=2:length(term_mat(:,1)),
  if term_mat(p,4)==4,
   den=conv(den,[1 term_mat(p,1)]);
  elseif term_mat(p,4)==5,
   num=conv(num,[1 term_mat(p,1)]);
  elseif term_mat(p,4)==6,
   den=conv(den,[1 2*term_mat(p,1)*term_mat(p,2) term_mat(p,2)^2]);
  elseif term_mat(p,4)==7,
   num=conv(num,[1 2*term_mat(p,1)*term_mat(p,2) term_mat(p,2)^2]);
  end
 end
else
 for p=2:lc,
  if term_mat(p,4)==2,
   pz=real(exp(-term_mat(p,1)*T));
   den=conv(den,[1 -pz]); nc=nc+1;
  elseif term_mat(p,4)==3,
   zz=real(exp(-term_mat(p,1)*T));
   num=conv(num,[1 -zz]); dc=dc+1;
  elseif term_mat(p,4)==4,
   a=term_mat(p,1)*term_mat(p,2);
   b=term_mat(p,2)*sqrt(1-term_mat(p,1)^2);
   den=conv(den,[1 -2*exp(-a*T(1))*cos(b*T(1)) exp(-2*a*T(1))]);
   nc=nc+1;
  elseif term_mat(p,4)==5,
   a=term_mat(p,1)*term_mat(p,2);
   b=term_mat(p,2)*sqrt(1-term_mat(p,1)^2);
   num=conv(num,[1 -2*exp(-a*T(1))*cos(b*T(1)) exp(-2*a*T(1))]);
   dc=dc+1;
  elseif term_mat(p,4)==7,
   if term_mat(p,1)==1, den=conv(den,[1 -1]); nc=nc+1;
   elseif term_mat(p,1)==2, den=conv(den,[1 -2 1]); nc=nc+1;
   elseif term_mat(p,1)==3, den=conv(den,[1 -3 3 -1]); num=conv(num,[1 1]);nc=nc+1; end
   if term_mat(p,2)==1, num=conv(num,[1 -1]); dc=dc+1;
   elseif term_mat(p,2)==2, num=conv(num,[1 -2 1]); dc=dc+1;
   elseif term_mat(p,2)==3, num=conv(num,[1 -3 3 -1]); den=conv(den,[1 1]);dc=dc+1; end
  end
 end
end

if nargin==2,
 t=nc-dc-term_mat(3,1);
 if t>0, num=conv(num,[1 zeros(1,t)]);
 elseif t<0, den=conv(den,[1 zeros(1,-t)]); end
else
 if term_mat(2,1)>0, den=conv(den,[1 zeros(1,term_mat(2,1))]);
 else num=conv(num,[1 zeros(1,abs(term_mat(2,1)))]); end
end
num = num*term_mat(1,1);

author: Sable

repo: mcbench-benchmarks


text:
function Q=QPolyKalDirectDisc(T,xDim,order,sigmaV2)
%%QPOLYKALDIRECTDISC Get the process noise covariance matrix for a direct
%           discrete linear dynamic model of the given polynomial
%           order (number of derivatives of position included) and number
%           of dimensions (generally 3 for 3D motion). order=1 means
%           the discrete white noise acceleration model (DWNA). order=2
%           means the discrete white noise jerk model, etc. The state is
%           ordered in terms of position, velocity, acceleration, etc.
%           Unlike the discretized continuous-time model in the function
%           QPolyKal, the direct-discrete implementation produces a
%           singular matrix and inconsistencies arise when predicting over
%           a time period of 2*T versus sequentially predicting over two
%           time perods of length T. In this function, the process noise is
%           modeled as being one moment higher than the highest order of
%           the state. This is contrasted with the function
%           QPolyKalDirectAlt where the process noise is at the highest
%           order of the state.
%
%INPUTS: T The time-duration of the propagation interval.
%     xDim The dimensionality of the target state.
%          xDim is ((order+1)*numDim)X1 dimensional, where numDim is the
%          number of position dimensions of space (e.g. 2D or 3D).
%    order The order >=0 of the filter. If order=1, then it is constant
%          velocity, 2 means constant acceleration, 3 means constant jerk,
%          etc.
%  sigmaV2 The variance driving the process noise. This has units of
%          distance^2/time^(2*(order+1)). sqrt(sigmaV2)*T is proportional
%          to the maximum change in the highest-order moment of the system.
%          If a scalar is passed, it is assumed to be the same for all
%          dimensions. Otherwise, a numDimX1 or 1XnumDim vector should be
%          passed specifying the value for each dimension.
%
%OUTPUTS: Q The process noise covariance matrix under the direct discrete
%           linear dynamic model of the given order with motion in numDim
%           dimensions where the state is stacked
%           [position;velocity;acceleration;etc] where the number of
%           derivatives of position depends on the order given. order=0
%           means just position.
%
%Chapters 1.5.5 and 1.5.6 of [1] presents the discrete white noise
%acceleration and jerk models, for orders one and two. The logic behind the
%order 1 model, which extends to any order, is explained in Chapter 6.3.2
%of [2]. The generalization has the matrix for 1D motion being sigmaV2*G*G'
%where the ith element in the column vector G is T^(order-i+2)/(order-i+2)!
%where i counts from 1 to order+1.
%
%Note that order=2 produces the discrete white noise jerk model, not the
%discrete Wiener process acceleration model.
%
%This process noise matrix is most commonly used with the FPolyKal state
%transition matrix.
%
%REFERENCES:
%[1] Y. Bar-Shalom, P. K. Willett, and X. Tian, Tracking and Data Fusion.
%    Storrs, CT: YBS Publishing, 2011.
%[2] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%
%April 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDim=xDim/(order+1);

if(isscalar(sigmaV2))
    sigmaV2=ones(numDim,1)*sigmaV2;
end

numEl=order+1;
i=(1:numEl)';
G=T.^(order-i+2)./factorial(order-i+2);
%These are the elements of all of the 1D submatrices, except they have to
%be multiplied by the appropriate sigmaV.
QBase=G*G';

%Now, the elements just get spread across identity matrices (scaled by
%sigmaV2) that are numDim dimensional to form a process noise covariance
%matrix of the desired dimensionality. This is done using a Kronecker
%product.
Q=kron(QBase,diag(sigmaV2));

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.