Generate lifted form matrices for discrete state space system

I have a discrete recursive state space equation

x_{k+1}=A_dx_k+B_du_k

y_k=Cx_k+Du_k

where x0 is nx x 1, y is M x ny, u is N x nu.

I want to work with a single equation where y =f([g(theta) = A B C D] , x0 , u). explicit input-output form.

The program that I wrote for this already works and leverages a few tricks, but I'm sure that this is a very well studied problem in Control theory. I want to have a function that:

1. Names everything correctly (like Toeplitz matrix, Markov Parameter, Observability, convolution kernel)
2. Is as fast as possible
3. Can work as a function handle to preserve memory (sometimes my input data N is 100k long and that is not kind to my computer when Btheta can just be a matvec)

Ultimately I understand this program very well but not well enough that I think this is a good implementation. There must be some toolbox or app I am missing.

function [B_theta_c, y_theta_c, uc, I, A_theta, Hd, B_theta, yt] = toeplitzBtheta_FOCUSS_PreCalcs(y, theta, u, x0, sys_desc, choice, dat)

% Extract dimensions
[M, ny] = size(y);        % M = number of output samples, ny = number of outputs
[N, nu] = size(u);        % N = number of input samples, nu = number of inputs
nx = length(x0);          % nx = number of states


[Ad, Bd, Cd, Dd, d] = StateSpace(theta, sys_desc, dat, nx, ny, nu);

% Pre-indexing for downsampling
indices = calculateDownsamplingIndices(N, M);
CAk = powers_CAk(Ad, Cd, N+1);   % C*A^k powers are common to all three precalculations
CAk_needed = CAk(:, :, indices);
A_theta = reshape(permute(pagemtimes(CAk_needed, x0), [3 1 2]), M, ny);
Hd_step = reshape(permute(pagemtimes(CAk_needed, d),  [3 1 2]), M, ny);
Hd      = cumsum(Hd_step, 1);

B_theta_col = pagemtimes(CAk, Bd); % ny x nu x N+1
B_theta = zeros(ny*M, nu*N);

% Build B_theta row by row
% Fill in each row at once by dropping in the impulse array slice with the right delay
for i = 1:M
    ii = indices(i);                      % downsampled time for this output row
    BT_row_idx = (0:ny-1)*M + i;          % rows for all ny outputs at time i

    % --- Valid-lag mask (skip zeros): determine which input columns can affect output at time ii
    t_vec   = ii - (1:N);                 % 1×N
    j_valid = find((t_vec > 0) & (t_vec <= N));
    tj      = t_vec(j_valid);             % 1×K, K can be 0

    % Gather the (ny×nu)×K impulse slices for this row; empty is fine
    H_sel = B_theta_col(:, :, tj);        % ny × nu × K

    % --- Drop the entire ny×K block into place for each input channel (vectorized write)
    for c = 1:nu
        BT_col_idx = (c-1)*N + j_valid;
        vals = reshape(H_sel(:, c, :), ny, []);     % ny × K (or ny×0)
        B_theta(BT_row_idx, BT_col_idx) = vals; 
    end

    % % --- Drop the entire ny×K block into place for each input channel (vectorized write)
    % row_block = zeros(ny, nu*N);
    % cols = zeros(1, nu*numel(j_valid));
    % p = 1;
    % for k = 1:numel(j_valid)
    %     cols(p:p+nu-1) = (o:nu-1)*N + j_valid(k);
    %     p = p + nu;
    % end
    % row_block(:, cols) = H_sel(:,:);
    % B_theta(BT_row_idx, :) = row_block; % assign the whole row block at once

    % end
end     


% Get the corresponding B_theta matrix for the chosen component
start_col = (choice-1)*N + 1;
end_col = choice*N;
B_theta_c = B_theta(:, start_col:end_col);


%Calculate the contribution from all OTHER components
uc = u(:, choice); % extract chosen signal
% uc = u(choice, :)';
B_theta_u_others = B_theta * u(:) - B_theta_c * u(:, choice);
yt = y - A_theta - Hd;
y_theta_c = yt(:) - B_theta_u_others;

I = speye(size(B_theta_c, 1));

end



function CAk = powers_CAk(Ad, Cd, N)
% powers_CAk - Precompute C*A^k matrices efficiently
%
% This function precomputes C*A^k for k = 0 to N-1 using efficient algorithms.
% For 2x2 matrices, it uses the Cayley-Hamilton recurrence relation.
% For larger matrices, it uses direct matrix multiplication.

% Get matrix dimensions
[nx, ~] = size(Ad);
ny = size(Cd, 1);

% Initialize output matrix
CAk = zeros(ny, nx, N);

% Set first matrix: C*A^0 = C
CAk(:, :, 1) = Cd;
CAk(:, :, 2) = Cd * Ad;

% For 2x2 matrices, use Cayley-Hamilton recurrence for efficiency
% This avoids computing matrix powers directly
if nx == 2
    % Get trace and determinant for recurrence relation
    s1 = trace(Ad);   % s1 = λ1 + λ2
    s2 = det(Ad);     % s2 = λ1 * λ2

    % Use recurrence: A^k = s1*A^(k-1) - s2*A^(k-2)
    for k = 3:N
        CAk(:, :, k) = s1 * CAk(:, :, k-1) - s2 * CAk(:, :, k-2);
    end
else
    % For larger matrices, use direct multiplication
    % A^k = A^(k-1) * A
    for k = 3:N
        CAk(:, :, k) = CAk(:, :, k-1) * Ad;
    end
end

end

[Walter Roberson]

Is as fast as possible

You will need to exactly describe the computer system that the software is to be run on -- the exact MATLAB release, the exact CPU, the exact arrangement of cores, the exact amount of memory, the exact array sizes. Changing anything could result in the overall system no longer being as fast as possible. Running a different set of background tasks or other simultaneous tasks (for example, your browser... indeed the exact browser version) could result in the overall system no longer being as fast as possible. Remember that nanoseconds make a difference as to what is as fast as possible.

[Moses]

Alright sure, as fast as possible would semantically mean to call up Sam Altman and parfor on twelve thousand NVIDIA GPU's. I'm working towards as fast as possible within reason, generalizaility, and readability constraints.

I'm on the most recent release with an 8 core CPU. MIMO system with less than 10 i/o/initial conditions. Long I/O length at most 100k, with input length Nu = k*Ny. Google chrome is closed and I'm using light mode GUI.

[Torsten]

Could you explain in more detail what you mean by

I want to work with a single equation where y =f([g(theta) = A B C D] , x0 , u). explicit input-output form.

?

Could you explain inputs and outputs of your function toeplitzBtheta_FOCUSS_PreCalcs ?

[Moses]

It's a recursion problem

x_{k+1}=A_dx_k+B_du_k

y_k=Cx_k+Du_k

In this state system, y_k has to be determined be the current x_k and u_k. But x_k is directly ditermined by u_k and an initial x0. So I want to get a function for y_k in terms of only uk and x0. Then I want to stack all of those y_k's to get a y vector in terms of only x0 and u vector.

The coefficents for the state space, A B C D, are determined by some theta values. But that is not important here, only that their dimensions line up to be the proper dimensions for a state-input-output-feedthrough system.

So the relevant inputs for the function are y, theta, u, x0

And the relevant outputs are B_theta_c, y_theta_c = y - A_theta

Btheta and Atheta are the coeffecients for x0 and u such that y = Atheta*x0 + B_theta*u

The rest I return for debugging purposes only.

[Torsten]

Then I want to stack all of those y_k's to get a y vector in terms of only x0 and u vector.

...

So the relevant inputs for the function are y, theta, u, x0

Why is y an input to the function if you want to compute it ?

[Paul]

Perhaps I'm misunderstanding, but doesn't lsim do exactly this:

"So I want to get a function for y_k in terms of only uk and x0. Then I want to stack all of those y_k's to get a y vector in terms of only x0 and u vector."

For example:

A = diag([0.7,0.5]); B = rand(2,2); C = rand(2,2); D = zeros(2,2);
x0 = [1;2];
u = rand(5,2);  % five output samples
y = lsim(ss(A,B,C,D,-1),u,0:4,x0)

y = 5×2

2.8664 1.2577 1.7293 0.7715 1.4788 0.6733 1.3812 0.6379 1.2661 0.5904

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

One could wrap that all into a function if really wanted:

sysout = @(A,B,C,D,u,k,x0) lsim(ss(A,B,C,D,-1),u,0:4,x0);
y = sysout(A,B,C,D,u,0:4,x0)

y = 5×2

2.8664 1.2577 1.7293 0.7715 1.4788 0.6733 1.3812 0.6379 1.2661 0.5904

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

[Torsten]

If you are only interested in the y-vector and don't need the matrices A_theta and B_theta such that y = A_theta*x0 + B_theta*u, you shouldn't work with matrix powers, but only use the recursion (or lsim as suggested by @Paul ). It will be much faster.

rng("default")
A = diag([0.7,0.5]); B = rand(2,2); C = rand(2,2); D = zeros(2,2);
x0 = [1;2];
u = rand(2,5);  % five output samples
x_iter = x0;
y(1,:) = C*x_iter + D*u(:,1);
for i = 2:5
  x_iter = A*x_iter + B*u(:,i-1);
  y(i,:) = C*x_iter + D*u(:,i);
end
y

y = 5×2

1.1894 1.1913 1.7789 1.6595 1.5379 1.4484 1.8397 1.5497 1.8095 1.3427

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

y = lsim(ss(A,B,C,D,-1),u.',0:4,x0)

y = 5×2

1.1894 1.1913 1.7789 1.6595 1.5379 1.4484 1.8397 1.5497 1.8095 1.3427

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

[Moses]

@Torsten I'm not interested in the A_theta matrix, I only need it for y_theta -- but I need the B_theta matrix.

Before seeing these replies I just finished refactoring the A_theta part:

function [Phi_x0] = io_form(state_space, x0, M, N, Ts_u)


% Infer Ts_y from data 
% Data can be (M,N), (M-1,N-1), (M-1,N), (M,N-1)
% but time between samples is always an integer
if nargin < 5, Ts_u = 1; end 
% default to 1 microsecond/millisecond/second/minute/hour/day/week/fortnight/month/year/decade/jubilee/century/millennium etc. 
Ts_y = round(N / M * Ts_u);

% Compute Phi_x0
Phi_x0 = compute_phi_x0(state_space, M, x0, Ts_y);

% Compute Psi
% Psi = compute_psi(state_space, M, N, Ts_u, Ts_y);

end

function Phi_x0 = compute_phi_x0(state_space, M, x0, Ts_y)
% COMPUTE_PHI_X0 Computes initial state response

% Time vector at output sampling rate
t_y = (0:M-1)' * Ts_y;
Phi_x0 = initial(state_space, x0, t_y);
end

It was great to discover the initial() function. I'll check out your lsim() implementation now. The biggest issue stopping me from using these functions right away was that my input and output are sampled multirate - Ts_u = 1 and Ts_y = 10 for example.

I'm also working with data is that is often confused about its endpoints; one sample every minute for a day could mean (M,N) = any from 144,1441; 144,1440; 145,1441; 145,1440 - and I need to adapt to the data I am given,

I need the B_theta matrix for an iterative solver (reweighted least squares), so I think it would be best to just keep it in memory rather than functionize it.

I really appriciate the thoughtul answers - I'll check out lsim() - maybe there's a way to use it to get the B_theta matrix instead of just the reconstructed y. either way it's helpful becuase I was using the recusive approace to simulate my system later on, and it's better to just use lsim() if that's what it does.

I was also planning on using the impulse() function to get the markov parameters for B_theta instead of the current format I have for getting B_theta_col, then I'll try and do some linspace or meshgrid tricks to avoid that for loop.

[Torsten]

Here is the computation of A_theta and B_theta for the example from above:

rng("default")
A = diag([0.7,0.5]); B = rand(2,2); C = rand(2,2); D = zeros(2,2);
x0 = [1;2];
u = rand(2,5);  % five output samples
C5 = repmat({C},1,5);
D5 = repmat({D},1,5);
A_theta = blkdiag(C5{:})*[eye(size(A));A;A^2;A^3;A^4];
B_theta = blkdiag(C5{:})*[zeros(2,10);[B,zeros(2,8)];[A^1*B,B,zeros(2,6)];[A^2*B,A^1*B,B,zeros(2,4)];[A^3*B,A^2*B,A^1*B,B,zeros(2,2)]] + ...
          blkdiag(D5{:});
y = A_theta*x0 + B_theta*u(:)

y = 10×1

1.1894 1.1913 1.7789 1.6595 1.5379 1.4484 1.8397 1.5497 1.8095 1.3427

<mw-icon class=""></mw-icon>

<mw-icon class=""></mw-icon>

Do you see the pattern and can you transfer it to the general case ?

Can you deduce an efficient code to compute the matrices involved (A,A^2,...,A^4,A*B,A^2*B,A^3*B)?

[Moses]

Yes, that's not the hard part -- Calculating the successive powers involved was a part of my old program (the powers_CAk function plus that last line with pagemtimes)

[nx, ~] = size(Ad);
ny = size(Cd, 1);

% Initialize output matrix
CAk = zeros(ny, nx, N);

% Set first matrix: C*A^0 = C
CAk(:, :, 1) = Cd;
CAk(:, :, 2) = Cd * Ad;

% For 2x2 matrices, use Cayley-Hamilton recurrence for efficiency
% This avoids computing matrix powers directly
if nx == 2
    % Get trace and determinant for recurrence relation
    s1 = trace(Ad);   % s1 = λ1 + λ2
    s2 = det(Ad);     % s2 = λ1 * λ2

    % Use recurrence: A^k = s1*A^(k-1) - s2*A^(k-2)
    for k = 3:N
        CAk(:, :, k) = s1 * CAk(:, :, k-1) - s2 * CAk(:, :, k-2);
    end
else
    % For larger matrices, use direct multiplication
    % A^k = A^(k-1) * A
    for k = 3:N
        CAk(:, :, k) = CAk(:, :, k-1) * Ad;
    end
end


B_theta_col = pagemtimes(CAk, Bd);

Right now I'm just using the command

h = impulse(sys_d, Kmax);

Where sys-d is the discritized state space and Kmax is a truncated impulse horizon (can be considered to be N)

This gives me All of the C*A^k*B powers I need.

The problem I have is with forming that final matrix. I know the structure: lower triangular Toeplitz (meaning every diagonal is the same and the upper triangle is all zeros), with a certain sampling rate such that rows are removed from the full toeplitz matrix (or never created).

My current implementation to get B_Theta costs a big M long for loop (M could be up to 100k long)

B_theta = zeros(ny*M, nu*N);

% Build B_theta row by row
% Fill in each row at once by dropping in the impulse array slice with the right delay
for i = 1:M
    ii = indices(i);                      % downsampled time for this output row
    BT_row_idx = (0:ny-1)*M + i;          % rows for all ny outputs at time i

    % --- Valid-lag mask (skip zeros): determine which input columns can affect output at time ii
    t_vec   = ii - (1:N);                 % 1×N
    j_valid = find((t_vec > 0) & (t_vec <= N));
    tj      = t_vec(j_valid);             % 1×K, K can be 0

    % Gather the (ny×nu)×K impulse slices for this row; empty is fine
    H_sel = h(:, :, tj);        % ny × nu × K

    % --- Drop the entire ny×K block into place for each input channel (vectorized write)
    for c = 1:nu
        BT_col_idx = (c-1)*N + j_valid;
        vals = reshape(H_sel(:, c, :), ny, []);     % ny × K (or ny×0)
        B_theta(BT_row_idx, BT_col_idx) = vals; 
    end
end

SO in summary, I've solved the challenge of computing A_theta*x0 and B_theta_col, but efficiently forming the full B_theta matrix is vexing me.

I'm especially not sure if I want the matrix is a block-toeplitz form (where each ny or nu row/column is stacked to keep the matrix 2D, (M*ny, N*nu)) or in tensor form where there is an M*N matrix that contains elements of dimension ny*nu

I need the B_theta matrix to multiply against other matricies later, so I'm leaning toward keeping block_toeplitz.

Also like I said M can be very large - so I a mtrying to avoid blkdiag and repmat, and instead using the sparse() matrix command.