# MATLAB Code Dump

[TOC]

## Algorithms

### EmpGram.m

function Wo = EmpGram(f, g, n, dt, T, x0, eps)
% Li Kin Fung, February 2024
% This is a function that calculates the empirical observability Gramian by simulating the perturbed systems.
% The implementation is based on 2015 "Empirical Observability Gramian Rank Condition for Weak Observability of Nonlinear Systems with Control"
% by Powel and Morgansen.
% Usage: [Wo, x_pis, y_pis] = EmpGram(f, g, n, dt, T, x0, eps)
%   Inputs:
%   - f: system map
%   - g: output map
%   - n: number of state dimensions
%   - dt: time step
%   - T: simulation period
%   - x0: initial state
%   - eps: magnitude of perturbation
%   Output:
%   - Wo: Empirical observability Gramian

tspan = 0:dt:T;

% cells to keep perturbed state and output trajectories
x_pis = cell(n, 1);
x_nis = cell(n, 1);
y_pis = cell(n, 1);
y_nis = cell(n, 1);

%% run 2n simulations
for i=1:n
    e_i = double(1:n == i)';  % i-th standard basis vector
    [~, x_pis{i}] = ode45(f, tspan, x0 + eps*e_i);
    [~, x_nis{i}] = ode45(f, tspan, x0 - eps*e_i);

    % dim-1 output only
    y_pis{i} = zeros(numel(tspan), 1);
    y_nis{i} = zeros(numel(tspan), 1);

    for k = 1:numel(tspan)
        y_pis{i}(k) = g(x_pis{i}(k, :));
        y_nis{i}(k) = g(x_nis{i}(k, :));
    end
end

%% Compute the Phi matrix
Phis = cell(numel(tspan), 1);
for k = 1:numel(tspan)
    % works for 1-dimensional output only
    Phi = zeros(1, n);
    for i=1:n
        Phi(:, i) = y_pis{i}(k) - y_nis{i}(k);
    end
    Phis{k} = Phi;
end

%% Integration by trapezoidal rule
Wo = zeros(n, n);
for k = 1:(numel(tspan)-1)
    % trapezoidal rule: times width (step size), divide by 2
    Wo = Wo + (Phis{k}' * Phis{k} + Phis{k+1}' * Phis{k+1});
end
Wo = Wo * dt / 2 / (4*eps^2);

### GaussianKernel.m

function kernel = GaussianKernel(x, y, sigma)
    % Compute the Gaussian kernel according to
    % k(\gamma_{a}, \gamma_{b})
    % = \exp[-(1/2\sigma^2)\sum_{t=0}^{T}\sum_{i=1}^{n_{y}}(\gamma_{a,i,t}-\gamma_{b,i,t})^{2}]

    % the last dimension is the axis for each data point
    % Compute the Eudlidean distance matrix between x and y
    n = size(x, 3);
    d = zeros(n, n);
    for i = 1:n
        for j = 1:n
            % Calculate the norm of the difference
            d(i, j) = norm(x(:,:,i) - y(:,:,j));
        end
    end
    
    % Calculate the kernel value using the Gaussian formula
    kernel = exp(-d.^2/sigma^2/2);
end

### MMD2Estimate.m

function estimate = MMD2Estimate(x, y, kernel)
    % Li Kin Fung, April 2024
    % This is a MATLAB version of the original code by Massiani (2023)
    % Usage: estimate = MMD2Estimate(x, y, kernel)
    %   Inputs:
    %   - x, y: data to compare
    %   - k: positive definite kernel
    
    % Compute the kernel terms
    k_xx = kernel(x, x);
    k_yy = kernel(y, y);
    k_xy = kernel(x, y);

    % If size(x, 1) (resp. size(y, 1)) is 0, then the terms in 1/n (resp. 1/m) are 0
    % in the final sum. So we set n (resp. m) to 1 to avoid numerical issues
    n = max(size(x, 1), 1);
    m = max(size(y, 1), 1);
    
    % Compute the MMD metric
    estimate = sum(k_xx, 'all') / n^2 + sum(k_yy, 'all') / m^2 - sum(k_xy, 'all') * 2 / (n*m);
end

### MMDThreshold.m

function kappa = MMDThreshold(K, m, alpha)
% Li Kin Fung, April 2024
% This function computes the rejection threshold for the MMD test.
% See Proposition 2 of "Data-Driven Observability Analysis for Nonlinear Systems"
% by Massiani et al.
% Usage: kappa = MMDThreshold(K, m, alpha)
%   Inputs:
%   - K: upper bound of the kernel function
%   - m: number of trajectories
%   - alpha: type I risk
%   Output:
%   - kappa: threshold below which the null hypothesis can be rejected
    kappa = sqrt(2*K/m) * (1 + sqrt(2*log(1/alpha)));
end

### MMDTest.m

%% Define the system
% dX_{t} = AX_{t}dt + A_{0}\sin(\omega t)dt + \Sigma dW_{t}
% Y_{t} = CX_{t} + \epsilon_{t}
A = [-2, -1; -1, -2];  % eig values -1, -3 with eig vecs [-1,1] and [1,1]
A0 = [3; 3];
omega = 2;
C = [-1, 1];
Sigma = 0.1 * eye(2);
epsilon_mean = 0;
epsilon_var = 0.01;  % measurement noise

%% Simulation settings
xa = [1.5; 0.5];  % reference point
n_grid = 50;  % number of grid points on one axis
n_traj = 30;  % number of trajectories
T = 200;  % length of trajectory
dt = 0.01;
tspan = linspace(0, dt*T, T);  % 200 points in [0s, 2s]

rng(1);  % set random seed for reproducibility

% Preallocate arrays to store trajectories
Xa = zeros(2, T, n_traj);
Ya = zeros(1, T, n_traj);
Xb = zeros(2, T, n_traj);
Yb = zeros(1, T, n_traj);

%% Simulate the trajectories from reference point x_a
for i = 1:n_traj
    Xa(:, 1, i) = xa;
    
    for t = 1:T-1
        dW = sqrt(dt) * randn(2, 1);
        dt_sin = A0 * sin(omega * t) * dt;
        dX = A * Xa(:, t, i) * dt + dt_sin + Sigma * dW;
        Xa(:, t+1, i) = Xa(:, t, i) + dX;
        Ya(:, t, i) = C * Xa(:, t, i) + sqrt(epsilon_var) * randn(1) + epsilon_mean;
    end
end

%% Simulate the trajectories from each x_b in the grid
% Define the range of x_b
xb_xrange = linspace(-2, 2, 50);
xb_yrange = linspace(-2, 2, 50);

% % Display the grid
% scatter(X(:), Y(:), 'filled');
% axis equal;

mmd_map = zeros(n_grid^2, 1);
num_iter = 1;
for x = xb_xrange
    for y = xb_yrange
        xb = [x; y];
        for i = 1:n_traj
            Xb(:, 1, i) = xb;
            
            for t = 1:T-1
                dW = sqrt(dt) * randn(2, 1);
                dt_sin = A0 * sin(omega * t) * dt;
                dX = A * Xb(:, t, i) * dt + dt_sin + Sigma * dW;
                Xb(:, t+1, i) = Xb(:, t, i) + dX;
                Yb(:, t, i) = C * Xb(:, t, i) + sqrt(epsilon_var) * randn(1) + epsilon_mean;
            end
        end

        % compute MMD metric
        sigma = 100;
        mmd_map(num_iter) = sqrt(MMD2Estimate(Ya, Yb, @(x,y) GaussianKernel(x, y, sigma)));
        num_iter = num_iter + 1;
    end
end

%% Plot the trajectories from xa=(1.5,0.5) and xb=(2,2)
figure
hold on
for i=1:n_traj
    plot(tspan, Ya(:,:,i));
    plot(tspan, Yb(:,:,i));
end
title('Trajectories from x_a=(1.5,0.5) and x_b=(2,2)')
xlabel('t')
ylabel('y')

%% Plot the MMD over the xb mesh in 3D
% Mesh of x_b
[xb_xq, xb_yq] = meshgrid(xb_xrange, xb_yrange);
xb_mesh = [xb_xq(:), xb_yq(:)];
vq = griddata(xb_mesh(:,1), xb_mesh(:,2), mmd_map, xb_xq, xb_yq);
figure
mesh(xb_xq,xb_yq,vq)
hold on
plot3(xb_mesh(:,1), xb_mesh(:,2), mmd_map, 'o')

% Plot xa and its class of indistinguishability
% xa
offset = 0.2;  % for visualization purpose only
xa_3d = [xa; offset];  
plot3(xa_3d(1), xa_3d(2), xa_3d(3), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
% line
line_start = [-1, -2, offset];
line_end = [2, 1, offset];
line_coords = [line_start; line_end];
plot3(line_coords(:, 1), line_coords(:, 2), line_coords(:, 3), 'r-', 'LineWidth', 3);

xlim([-2 2])
ylim([-2 2])
title('MMD_b[n, m]')
xlabel('x_{b,1}')
ylabel('x_{b,2}')
zlabel('MMD')

%% Plot the class of indistinguishability as a scatterplot
% K=1 as Gaussian kernel must be <= 1
alpha = 0.05;
kappa = MMDThreshold(1, n_traj, alpha);
points = xb_mesh(vq < kappa, :);  % class of indistinguishability
figure
scatter(points(:, 1), points(:, 2))
title('Class of Indistinguishability (MMD_b < \kappa) relative to x_a')
xlabel('x_{b,1}')
ylabel('x_{b,2}')

### ETOutput.m

function [y_tau, taus] = ETOutput(y, delta)

% Li Kin Fung, March 2024
% Event-triggered output code
% inputs: y (sampled output), delta (event trigger threshold)
% outputs: y_tau (event-triggered output), taus (triggering times)
% NOTE: We will restrict ourselves to single, vector-valued output in this work

% Initialize variables
numIterations = length(y);
y_tau = zeros(numIterations, 1);  % Sequence of output values exceeding threshold
taus = zeros(numIterations, 1);  % Sequence of event instants

% Initial conditions
y_tau(1) = y(1);
taus(1) = 1;

% Compute event-triggered output
for k = 2:numIterations
    % Check if output exceeds the threshold
    if abs(y(k) - y_tau(k-1)) > delta
        y_tau(k) = y(k);
        taus(k) = k;
    else
        y_tau(k) = y_tau(k-1);
        taus(k) = taus(k-1);
    end
end

### ETInitialStateReconstruction.m

function [x0hat, y_tau, taus] = ETInitialStateReconstruction(sys, x0, n, T, delta, regularization)
% Li Kin Fung, April 2024
% Event-triggered initial state reconstruction code
% inputs: sys (discrete system), x0 (true initial state), n (number of states),
%         T (simulation time), delta (event trigger threshold),
%         regularization ('l1', 'l2' or others)
% outputs: x0hat (reconstructed initial state), y_tau (event-triggered output), taus (triggering times)
% NOTE: We will restrict ourselves to single, vector-valued output in this work

%% Define system parameters and variables
A = sys.A;
C = sys.C;
tspan = 1:T;  % Time from 0 to T
y = lsim(sys, zeros(size(tspan)), tspan, x0);

%% Obtain ET output and instants
[y_tau, taus] = ETOutput(y, delta);

%% Optimization based on Event-triggered Outputs
% Initialize optimization variables
x0hat = sdpvar(n, 1);

% Formulate the optimization problem
Constraints = [];
Mo = [];
for i = 1:length(taus)
    % Add constraint based on event trigger
    if taus(i) == i
        % i is an event instant
        Constraints = [Constraints; y(i) == C*A^(i-1)*x0hat];
    else
        % i is a no-event instant
        Constraints = [Constraints; abs(C*A^(i-1)*x0hat - y(taus(i))) <= delta];
    end
    % Construct the observability matrix
    Mo = [Mo; C * (A^(i-1))];
end

% Objective function to minimize
Objective = norm(y_tau - Mo * x0hat, 2);

%% Optionally add a p-norm regularization (p=1,2)
if strcmp(regularization, 'l1')
    bound = sdpvar(n, 1);
    Constraints = [Constraints; -bound <= x0hat <= bound];
    Objective = Objective + sum(bound);
elseif strcmp(regularization, 'l2')
    bound = sdpvar(n, 1);
    Constraints = [Constraints; -bound <= x0hat <= bound];
    Objective = Objective + norm(bound, 2);
end

%% Solve optimization problem
solution = optimize(Constraints, Objective);

%% Convert x0hat from sdpvar to vector
x0hat = double(x0hat);

## Code for Worked Examples

### wang2008state.m

clear all, close all, clc

% Li Kin Fung, January 2024
% Example from paper:
% "State Reconstruction for Linear Time-invariant Systems with Binary-valued Output Observations"
% by Le Yi Wang, Guohua Xu and G. Yin
% Example 1

% system dynamics
A = [0 1; -2 -2];
B = [0; 1];
C = [1 0];

x0 = [1; 0];  % initial state
dt = 0.0001;  % time step size = 0.0001s
tspan = 0:dt:3.9; % simulate from t=0 to 3.9 seconds

T = 0.5; % threshold value
u_condition = @(t, y) (C * y > T) * -1 + (C * y <= T) * 1;  % input function

% define and solve the system
systemDynamics = @(t, x) A * x + B * u_condition(t, x);
[t, x] = ode45(systemDynamics, tspan, x0);

% Compute the system and sensor outputs, and control input
y = C * x';
s = y > T;
u = (y > T) * -1 + (y <= T) * 1;  % input values, different from input function

% Plot the results
figure;
subplot(3, 1, 1);
plot(t, u, 'r');
ylabel('control input');
grid on

subplot(3, 1, 2);
plot(t, x(:, 1), 'b--');
hold on;
plot(t, x(:, 2), 'r-');
ylabel('state trajectory');
legend({'x_1','x_2'},'Location','southeast');
grid on

subplot(3, 1, 3);
plot(t, y, 'b--');
hold on;
plot(t, s, 'r');
hold on;
plot(t, T * ones(size(y)), 'k');
xlabel('time (second)');
ylabel('output and sensor trajectories');
legend('system output y', 'sensor output', 'threshold');
grid on

% Find the switching time(s) of control input
switchIndex = (find(diff(u) ~= 0) + 1);
ts = tspan(switchIndex);  % switching time

% state reconstruction
% M is mn*p, where C is m*k, A is k*p, and there are n switching times
M = zeros(size(C, 1)*numel(ts), size(A, 2));
for i = 1:numel(ts)
    rowstart = size(C, 1) * i;
    rowend = size(C, 1) * (i+1) - 1;
    M(rowstart:rowend, :) = C * expm(A*ts(i));
end

% Initialize the integral result
beta = zeros(size(B));
% trapezoidal rule approximation
for i = 1:size(ts, 2)
    for k = 1:switchIndex(i)
        tau = tspan(k);
        % this is not exactly beta
        beta(i) = beta(i) + C * (expm(A*(ts(i)-tau)) + expm(A*(ts(i)-(tau+dt)))) * B * u(k);
    end
end
xiTilde = beta * dt / 2;  % trapezoidal rule: times width, then divide by 2
xi = T - xiTilde;
x0hat = linsolve(M, xi);  % xi = M*x0hat

% report the results
disp("Eigenvalues of A:");
disp(eig(A));
disp("Switching time(s):")
disp(ts);
disp("M:")
disp(M);
disp("beta(ts):")
disp(beta);
disp("Reconstructed initial state x0hat:")
disp(x0hat);

### powel2015empirical.m

% Example from paper:
% "Empirical Observability Gramian Rank Condition for Weak Observability of Nonlinear Systems with Control"
% by Nathan D. Powel and Kristi A. Morgansen

%% Section 4 Example A. Nearly linear
% The nonlinear system is
% x1 dot = -x2
% x2 dot = x1*u
% y = x2
% Linearization at equilibrium (x1,x2)=(0,0) (in deviation variables) gives:
% x'=Ax, y=C where

n = 2;  % number of dimensions of state space

%% 1. Linear Gramian at equilibrium with control u=1
% Define the system matrices
A = [0 -1; 1 0];
C = [0 1];
sys = ss(A, [], C, []);

O = obsv(A, C);         % observability matrix
% or: O = obsv(sys);
[U, S, V] = svd(O);     % SVD of O
rank_O = rank(O);
% cannot compute linear Gramian due to unstable dynamics
disp("Method 1: Linear Gramian at equilibrium with control u=1");
disp("Observability matrix O:");
disp(O);
disp("Rank of O:");
disp(rank_O);

%% 2. Empirical Gramian in Powel and Morgansen (2015), with u=1 and x0=[1;0]
x0 = [1; 0];  % initial state
eps = 0.01;  % epsilon
dt = 0.01;  % time step
T = 10;  % simulation period
f = @(~, x) [-x(2); x(1)];
g = @(x) x(2);
Wo_emp = EmpGram(f, g, n, dt, T, x0, eps);

disp("Method 2: Empirical Gramian in Powel and Morgansen (2015), with u=1 and x0=[1;0]");
disp("Empirical Gramian Wo_emp (eps=0.01):");
disp(Wo_emp);
disp("Condition number:");
disp(cond(Wo_emp));