function [T, R_lo, R_hi, center] = reach_linear(A_cl, B_w, x0_center, x0_halfwidth, w_lo, w_hi, tspan, dt) % REACH_LINEAR Interval reach set for dx/dt = A_cl*x + B_w*w, w in [w_lo, w_hi]. % % Hand-rolled zonotope propagator specialized to the case where: % - The initial set is an axis-aligned box around x0_center with % halfwidths x0_halfwidth. % - The disturbance w is a scalar interval. % % The reach set at time t is: % X(t) = {expm(A_cl*t) * x0 + integral_0^t expm(A_cl*(t-s))*B_w*w(s) ds : % x0 in X0, w(s) in W}. % % Decompose: % X(t) = expm(A_cl*t) * X0 (+) S_w(t), % where S_w(t) is the reachable contribution of the disturbance. % Both parts are zonotopes; we over-approximate each by a box at every % time step via the matrix/integral absolute sum (interval hull). % % Inputs: % A_cl - n x n closed-loop state matrix (Hurwitz assumed) % B_w - n x 1 disturbance vector % x0_center - n x 1 center of initial set % x0_halfwidth - n x 1 halfwidths of initial set (>=0) % w_lo, w_hi - scalar disturbance bounds % tspan - [t0, tf] % dt - time step for exporting the reach tube (s) % % Outputs: % T - M x 1 time grid % R_lo - n x M lower bounds of reach set % R_hi - n x M upper bounds of reach set % center- n x M center trajectory (nominal, w = w_mid) n = size(A_cl, 1); assert(size(A_cl,2) == n, 'A_cl must be square'); assert(numel(x0_center) == n && numel(x0_halfwidth) == n, 'x0 dim mismatch'); x0_center = x0_center(:); x0_halfwidth = x0_halfwidth(:); B_w = B_w(:); w_mid = 0.5 * (w_lo + w_hi); w_half = 0.5 * (w_hi - w_lo); T = (tspan(1):dt:tspan(2)).'; M = numel(T); R_lo = zeros(n, M); R_hi = zeros(n, M); center = zeros(n, M); % Disturbance contribution: % S_center(t) = int_0^t expm(A*(t-s))*B_w*w_mid ds (signed, centerline) % S_half(t) = int_0^t |expm(A*(t-s))*B_w| * w_half ds (elementwise, halfwidth) % % The halfwidth uses elementwise |.| because under an axis-aligned % box over-approximation, |linear-map| contributes absolute values. % This is the interval-arithmetic analog of propagating a zonotope. S_center = zeros(n, 1); S_half = zeros(n, 1); x_center_now = x0_center; A_step = expm(A_cl * dt); % state transition per step A_abs_step = abs(A_step); % for interval-halfwidth % Integrated input gain per step: % G_step = int_0^dt expm(A*s)*B_w ds % Augmented-matrix trick: upper-right block of expm([A B_w; 0 0]*dt). M_aug = expm([A_cl, B_w; zeros(1, n+1)] * dt); G_step = M_aug(1:n, n+1); % n x 1 G_abs_step = abs(G_step); halfwidth_now = x0_halfwidth; % Initial state center(:, 1) = x_center_now + S_center; R_lo(:, 1) = center(:, 1) - halfwidth_now - S_half; R_hi(:, 1) = center(:, 1) + halfwidth_now + S_half; for k = 2:M % State piece x_center_now = A_step * x_center_now; halfwidth_now = A_abs_step * halfwidth_now; % Disturbance piece S_center = A_step * S_center + G_step * w_mid; S_half = A_abs_step * S_half + G_abs_step * w_half; center(:, k) = x_center_now + S_center; R_lo(:, k) = center(:, k) - halfwidth_now - S_half; R_hi(:, k) = center(:, k) + halfwidth_now + S_half; end end