Output-COSMIC: Extension of COSMIC (Closed-form Optimal data-driven linear time-varying SysteM IdentifiCation) to Partial State Observation¶

1. Problem Statement¶

Consider a discrete-time linear time-varying (LTV) system with partial state observation:

\[ x(k+1) = A(k)\,x(k) + B(k)\,u(k), \qquad k = 0, \ldots, N-1 \]

\[ y(k) = H\,x(k) \]

where $x(k) \in \mathbb{R}^n$ is the state, $u(k) \in \mathbb{R}^q$ is a known input, $y(k) \in \mathbb{R}^{p_y}$ is the measurement, and $H \in \mathbb{R}^{p_y \times n}$ is a known, time-invariant observation matrix. The system matrices $A(k) \in \mathbb{R}^{n \times n}$ and $B(k) \in \mathbb{R}^{n \times q}$ are unknown and to be estimated.

Standard COSMIC assumes $H = I$ (full state observation). This document extends COSMIC to partial observations ($p_y \leq n$) and over-determined observations ($p_y > n$). When $\operatorname{rank}(H) = n$ (including $p_y \geq n$), the state is exactly recoverable via weighted least squares and no iterative estimation is needed (see Section 4.1, Case 1).

2. Joint Objective¶

Define the combined dynamics matrix $C(k) = \begin{bmatrix} A^\top(k) \\ B^\top(k) \end{bmatrix} \in \mathbb{R}^{(n+q) \times n}$ and the augmented data vector $d(k) = \begin{bmatrix} x(k) \\ u(k) \end{bmatrix} \in \mathbb{R}^{n+q}$.

The joint objective over the state sequence $\mathbf{x} = \{x(k)\}_{k=0}^{N}$ and the dynamics sequence $\mathbf{C} = \{C(k)\}_{k=0}^{N-1}$ is:

\[ J(\mathbf{x}, \mathbf{C}) = \sum_{k=0}^{N} \|y(k) - H\,x(k)\|^2_{R^{-1}} + \sum_{k=0}^{N-1} \|x(k+1) - A(k)\,x(k) - B(k)\,u(k)\|^2 + \lambda \sum_{k=1}^{N-1} \|C(k) - C(k-1)\|_F^2 \]

where $\|v\|^2_{R^{-1}} = v^\top R^{-1} v$ denotes the Mahalanobis norm with respect to the measurement noise covariance $R \in \mathbb{R}^{p_y \times p_y}$ (symmetric positive definite), and:

Observation fidelity ($R^{-1}$ term): penalises deviation of the estimated state from measurements, weighted by the measurement information matrix $R^{-1}$. This naturally downweights noisy measurement channels and upweights precise ones. If $R$ is unknown, setting $R = I$ recovers unweighted least squares.
Dynamics fidelity (middle term): penalises deviation from the estimated dynamics model, coupling $\mathbf{x}$ and $\mathbf{C}$.
Dynamics smoothness ($\lambda$ term): the standard COSMIC regulariser, penalising rapid variation of the system matrices between consecutive time steps.

2.1 Recovery of Standard COSMIC¶

When $H = I$ (full state observation), taking $R \to 0$ (infinite measurement precision) forces $x(k) = y(k)$ exactly. The observation fidelity term vanishes (it is zero at the constraint), the state is no longer a free variable, and $J$ reduces to the standard COSMIC cost:

\[ J(\mathbf{C}) = \sum_{k=0}^{N-1} \|y(k+1) - C^\top(k)\,d(k)\|^2 + \lambda \sum_{k=1}^{N-1} \|C(k) - C(k-1)\|_F^2 \]

which is exactly the original COSMIC formulation of Carvalho et al. (2022). No additional hyperparameters are introduced in the fully-observed case.

3. Multi-Trajectory Extension¶

Assume $L$ trajectories are collected, each of length $N+1$ time steps, under the same LTV dynamics $\{A(k), B(k)\}_{k=0}^{N-1}$. Denote the state and output of trajectory $l$ at time $k$ as $x_l(k)$ and $y_l(k)$ respectively. The joint objective generalises to:

\[ J(\mathbf{X}, \mathbf{C}) = \sum_{l=1}^{L} \sum_{k=0}^{N} \|y_l(k) - H\,x_l(k)\|^2_{R^{-1}} + \sum_{l=1}^{L} \sum_{k=0}^{N-1} \|x_l(k+1) - A(k)\,x_l(k) - B(k)\,u_l(k)\|^2 + \lambda \sum_{k=1}^{N-1} \|C(k) - C(k-1)\|_F^2 \]

The dynamics smoothness term is shared across trajectories (single $\mathbf{C}$), while the observation and dynamics fidelity terms are summed over trajectories. Each trajectory has its own state sequence $\{x_l(k)\}$ but shares the same $\{C(k)\}$.

4. Alternating Minimisation Algorithm¶

The objective $J(\mathbf{x}, \mathbf{C})$ is non-convex jointly (due to the bilinear coupling $A(k)\,x(k)$), but is strictly convex in each block separately when the other is held fixed (given $\lambda > 0$ and $R$ positive definite). This motivates a two-block alternating minimisation scheme.

4.1 Initialisation¶

The initialisation strategy depends on the rank of $H$.

Case 1: $\operatorname{rank}(H) = n$ (full-rank fast path). When $H$ has full column rank (including $H = I$ and tall matrices with $p_y > n$), the state $x(k)$ is exactly recoverable from $y(k)$ via weighted least squares:

\[ \hat{x}(k) = (H^\top R^{-1} H)^{-1} H^\top R^{-1}\, y(k) \]

This eliminates the state as a free variable. A single COSMIC step on the recovered states produces the final $A(k)$, $B(k)$ — no alternating loop is needed. The observation fidelity term achieves its minimum at the weighted LS solution for each time step independently. The total cost is $O(N\,p_y\,n + N\,(n+q)^3)$, with no iterations.

Case 2: $\operatorname{rank}(H) < n$ (partial observation). When $H$ is rank-deficient, the state cannot be recovered from measurements alone. The algorithm uses an LTI frequency-domain initialisation followed by alternating minimisation:

LTI initialisation (sidLTIfreqIO). Estimate constant dynamics $(A_0, B_0)$ from the I/O transfer function $G(e^{j\omega}) = H(e^{j\omega}I - A_0)^{-1}B_0$ via Blackman-Tukey spectral estimation and Ho-Kalman realization. The realization is transformed to the $H$-basis so that $C_r = H$ in the observation equation. Set $A(k) = A_0$, $B(k) = B_0$ for all $k$. This provides an observable initialisation for any $H$.
Enter the alternating loop (Section 4.2).

Alternative initialisation (composite $A = I$ solve). When $H$ has full column rank, one can also evaluate $J$ at $A(k) = I$ and jointly solve for $\{x_l(k)\}$ and $\{B(k)\}$:

\[ J_{\text{init}}(\mathbf{X}, \mathbf{B}) = J(\mathbf{X}, \mathbf{C})\big|_{A(k)=I} = \sum_{l=1}^{L} \sum_{k=0}^{N} \|y_l(k) - H\,x_l(k)\|^2_{R^{-1}} + \sum_{l=1}^{L} \sum_{k=0}^{N-1} \|x_l(k+1) - x_l(k) - B(k)\,u_l(k)\|^2 + \lambda \sum_{k=1}^{N-1} \|B(k) - B(k-1)\|_F^2 \]

This is jointly convex (no bilinear terms). The minimiser is unique and obtained in a single forward-backward pass over composite blocks (Appendix B). Since $J_{\text{init}} = J\big|_{A=I}$, this is the exact minimisation of the global objective over a restricted subspace. However, the full-rank fast path above is strictly simpler (no composite block system needed) and produces the same result when followed by a COSMIC step. The composite $A = I$ solve cannot be used when $\operatorname{rank}(H) < n$ because the observability matrix $\mathcal{O} = [H;\, HA;\, HA^2;\, \ldots]$ with $A = I$ has rank $p_y < n$ and the composite system is structurally singular.

4.2 Alternating Loop¶

Starting from the initialisation, the algorithm alternates between two steps:

State Step. Fix $\mathbf{C} = \{C(k)\}$, solve for $\mathbf{x} = \{x(k)\}$:

\[ \min_{\mathbf{x}} \; \sum_{k=0}^{N} \|y(k) - H\,x(k)\|^2_{R^{-1}} + \sum_{k=0}^{N-1} \|x(k+1) - A(k)\,x(k) - B(k)\,u(k)\|^2 \]

This is a linear least-squares problem in $\{x(k)\}$. It is exactly the Rauch–Tung–Striebel (RTS) smoother with measurement noise covariance $R$ and process noise covariance $Q = I$, conditioned on the full observation sequence $\{y(k)\}_{k=0}^{N}$. The solution is unique and can be computed in $O(N\,n^3)$ time via the forward-backward recursion derived in Appendix A.

For multi-trajectory: the state step decouples across trajectories (each trajectory has its own state sequence but uses the shared $\mathbf{C}$), so $L$ independent smoothers are run.

COSMIC Step. Fix $\mathbf{x} = \{x(k)\}$ (or $\mathbf{X} = \{X(k)\}$ for multi-trajectory), solve for $\mathbf{C}$:

\[ \min_{\mathbf{C}} \; \sum_{k=0}^{N-1} \|x(k+1) - C^\top(k)\,d(k)\|^2 + \lambda \sum_{k=1}^{N-1} \|C(k) - C(k-1)\|_F^2 \]

This is exactly the standard COSMIC problem with $x(k)$ replacing the measured state. The observation fidelity term does not involve $\mathbf{C}$ and is therefore constant in this step. The solution is obtained via the COSMIC tridiagonal closed-form algorithm in $O(N\,(n+q)^3)$ time.

For multi-trajectory: the data matrices $D(k) = \begin{bmatrix} X(k) \\ U(k) \end{bmatrix}$ and targets $X'(k) = X(k+1)$ pool across trajectories exactly as in the standard multi-trajectory COSMIC formulation.

4.3 Trust-Region Interpolation (Optional)¶

In cases where the transition from the initialisation $(A_0, B_0)$ of Section 4.1 to the first COSMIC estimate of $A(k)$ is too abrupt — for instance with high noise, long trajectories, or poorly conditioned data — a trust-region mechanism can be employed.

Define the interpolated dynamics matrix, interpolating toward the LTI initialisation $A_0$ (not $I$; the $\mu I$ of earlier drafts predates the LTI initialisation — see SPEC §8.12.4 and issue #138):

\[ \tilde{A}(k) = (1 - \mu)\,A(k) + \mu\,A_0 \]

where $\mu \in [0, 1]$ is the trust-region parameter. The state step uses $\tilde{A}(k)$ in place of $A(k)$:

\[ \min_{\mathbf{x}} \; \sum_{k} \|y(k) - H\,x(k)\|^2_{R^{-1}} + \sum_{k} \|x(k+1) - \tilde{A}(k)\,x(k) - B(k)\,u(k)\|^2 \]

The COSMIC step is unaffected (it always solves for $A(k)$ and $B(k)$ freely).

Adaptive schedule. The trust-region parameter $\mu$ is managed by an outer loop wrapping the alternating minimisation:

This is an outer loop over $\mu$ that wraps the inner alternating loop of Section 4.2; the inner loop runs to its own stopping point before $\mu$ changes:

Initialise $\mu = 1$ (the first state step uses the $A_0$ initialisation).
Inner loop: run the alternating state–COSMIC loop at the fixed current $\mu$ until the relative cost change falls below $\varepsilon_J$ or the per-stage inner cap MaxIter is reached. Report $J^*(\mu)$ and $(X^*, C^*)_\mu$ as the best (lowest-cost) iterate observed during the stage — not necessarily the last, since for $\mu > 0$ the inner iteration is a homotopy fixed-point (not the monotone descent of Section 4.2), so a capped non-converged stage could otherwise report a worse $J^*$ than it visited.
Propose $\mu' = \mu / 2$; run the inner loop at $\mu'$, yielding $J^*(\mu')$.
Accept/reject: if $J^*(\mu') \leq J^*(\mu)$, accept ($\mu \leftarrow \mu'$, keep $(X^*, C^*)_{\mu'}$) and repeat from step 3. If $J^*(\mu') > J^*(\mu)$, reject — restore $(X^*, C^*)_\mu$ and stop reducing $\mu$.
Final $\mu = 0$ refinement (both exits). On leaving the outer loop — whether by reject (step 4) or by $\mu < \varepsilon_\mu$ (default $\varepsilon_\mu = 10^{-6}$) — run one inner loop at $\mu = 0$ from the current best iterate and keep its result only if it lowers $J^*$ (a guarded pass; costless in the worst case). Return that iterate. This guarantees the returned iterate is a fixed point of the reported ($\mu = 0$) alternation rather than one smoothed under $\tilde{A} \neq A$. MaxIter is a per-stage inner cap, so the worst-case total inner-iteration count is $\texttt{MaxIter} \times (\lceil \log_2(1/\varepsilon_\mu) \rceil + 2)$. See SPEC §8.12.4 for the normative budget and defaults.

At each $\mu > 0$ the inner loop recomputes $\tilde{A}(k) = (1-\mu)A(k) + \mu A_0$ from the latest COSMIC estimate $A(k)$ and iterates to a self-consistent fixed point. Because the state step's model $\tilde{A}$ then differs from the COSMIC step's free $A$, this inner iteration is a homotopy fixed-point and is not guaranteed monotone within a fixed $\mu$ (unlike the $\mu = 0$ loop of Section 4.2, Proposition 1). Monotonicity is instead enforced across $\mu$ by the outer accept/reject in step 4 — each accepted reduction lowers $J^*$. The outer loop is thus a monotone homotopy from the trusted LTI initialisation ($\mu = 1$) to the fully dynamics-informed model ($\mu = 0$).

When $\mu = 0$ throughout (the default): the trust-region mechanism is inactive, the outer loop is skipped, and the algorithm reduces to the base two-block alternation of Section 4.2, initialised from $(A_0, B_0)$. This is expected to be sufficient for most practical cases.

5. Convergence Analysis¶

5.1 Monotone Decrease¶

Proposition 1. Let $(\mathbf{x}^{(t)}, \mathbf{C}^{(t)})$ denote the iterates of the alternating minimisation (Section 4.2) at iteration $t$. Then the sequence $\{J(\mathbf{x}^{(t)}, \mathbf{C}^{(t)})\}$ is monotonically non-increasing.

Proof. At each iteration:

The state step sets $\mathbf{x}^{(t+1)} = \arg\min_{\mathbf{x}} J(\mathbf{x}, \mathbf{C}^{(t)})$, so $J(\mathbf{x}^{(t+1)}, \mathbf{C}^{(t)}) \leq J(\mathbf{x}^{(t)}, \mathbf{C}^{(t)})$.
The COSMIC step sets $\mathbf{C}^{(t+1)} = \arg\min_{\mathbf{C}} J(\mathbf{x}^{(t+1)}, \mathbf{C})$, so $J(\mathbf{x}^{(t+1)}, \mathbf{C}^{(t+1)}) \leq J(\mathbf{x}^{(t+1)}, \mathbf{C}^{(t)})$.

Combining: $J(\mathbf{x}^{(t+1)}, \mathbf{C}^{(t+1)}) \leq J(\mathbf{x}^{(t)}, \mathbf{C}^{(t)})$. Since $J \geq 0$, the sequence converges. $\square$

Effective smoothness weight (matches SPEC §8.12.2). Step 2 above is honest only if the COSMIC step actually minimises the same $J$. Because that step applies the $1/\sqrt{N}$ data scaling (§8.3.2), it minimises the dynamics fidelity plus smoothness with effective weight $N\lambda$, not $\lambda$. The single objective for which both steps are exact block minimisers — and hence the one that is monotone and is reported in the Cost field — therefore uses the effective weight: $$ J = \sum_k |y(k) - Hx(k)|^2_{R^{-1}} + \sum_k |x(k+1) - A(k)x(k) - B(k)u(k)|^2 + N\lambda \sum_k |C(k) - C(k-1)|_F^2. $$ The $\lambda$-weighted $J$ written in §2 is the user's nominal objective (same minimiser); the $N\lambda$ form above is what the alternating loop provably decreases. Reporting the $\lambda$-weighted value — as the previous implementation did — breaks the monotonicity guarantee of this proposition.

5.2 Convergence to a Stationary Point¶

Proposition 2. Every limit point of the sequence $\{(\mathbf{x}^{(t)}, \mathbf{C}^{(t)})\}$ is a stationary point of $J$.

Proof. Both subproblems are strictly convex (the state step has $R^{-1} \succ 0$ ensuring strict convexity in $\mathbf{x}$; the COSMIC step has $\lambda > 0$ ensuring strict convexity in $\mathbf{C}$), so each block minimiser is unique. This satisfies the conditions of the two-block alternating minimisation convergence theorem (Grippo and Sciandrone, 2000, Theorem 2.1): for two-block problems where each block minimiser is unique, every limit point of the alternating sequence is a stationary point of the joint objective. $\square$

5.3 Non-Convexity and Global Optimality¶

The joint objective $J(\mathbf{x}, \mathbf{C})$ is non-convex due to the bilinear term $A(k)\,x(k)$ in the dynamics fidelity. Multiple stationary points may exist. In particular, a similarity transformation $T \in \mathbb{R}^{n \times n}$ (invertible) maps any solution $(x(k), A(k), B(k))$ to an equivalent solution $(T\,x(k),\; T\,A(k)\,T^{-1},\; T\,B(k))$ with identical cost. The observation term $\|y(k) - H\,x(k)\|^2_{R^{-1}}$ breaks this symmetry partially (requiring $H\,T^{-1}$ to produce the same outputs), but does not eliminate it unless $H$ has full column rank.

Global optimality is therefore not guaranteed. The initialisation (Section 4.1) and optional trust-region mechanism (Section 4.3) serve to place the iterates in a favourable basin of attraction.

5.4 Trust-Region Convergence¶

Proposition 3. The outer trust-region loop (Section 4.3) produces a monotonically non-increasing sequence of converged objectives $\{J^*(\mu_s)\}$ as long as reductions are accepted.

Proof. At each outer iteration $s$, the inner alternating loop converges to a stationary point for fixed $\mu_s$ (Propositions 1–2). A reduction $\mu_{s+1} = \mu_s / 2$ is accepted only if $J^*(\mu_{s+1}) \leq J^*(\mu_s)$. If the reduction is rejected, the algorithm reverts to $\mu_s$ and terminates. Therefore, the accepted sequence of $J^*$ values is monotonically non-increasing, and the algorithm terminates in a finite number of outer iterations (since $\mu$ halves at each accepted step and terminates below $\varepsilon$). $\square$

6. Similarity Transformation Ambiguity¶

As noted in Section 5.3, the output-COSMIC problem admits equivalent solutions under state-space similarity transformations. For any invertible $T$:

\[ \tilde{x}(k) = T\,x(k), \quad \tilde{A}(k) = T\,A(k)\,T^{-1}, \quad \tilde{B}(k) = T\,B(k), \quad \tilde{H} = H\,T^{-1} \]

produces identical input-output behaviour. Since $H$ is fixed and known, the transformation is constrained by $H = \tilde{H}\,T = H\,T^{-1}\,T = H$, which is automatically satisfied — the point is that different initialisations may converge to different (but equivalent) state-space realisations.

This ambiguity is inherent to all output-based system identification methods and does not affect prediction or control design (which depend only on input-output behaviour). If a canonical form is desired, it can be imposed as a post-processing step (e.g., balanced realisation, observable canonical form).

7. Algorithm Summary¶

Algorithm: Output-COSMIC

Input: Measurements $\{y_l(k)\}$ for $l = 1, \ldots, L$, $k = 0, \ldots, N$; inputs $\{u_l(k)\}$ for $l = 1, \ldots, L$, $k = 0, \ldots, N-1$; observation matrix $H$; measurement noise covariance $R$ (or $R = I$ if unknown); regularisation weight $\lambda$; convergence tolerance $\varepsilon_J$; optional trust-region parameters $(\mu_0, \varepsilon_\mu)$.

Output: Estimated dynamics $\{A(k), B(k)\}_{k=0}^{N-1}$; estimated states $\{x_l(k)\}$.

Initialisation: 1. Solve $J_{\text{init}} = J\big|_{A=I}$ (Section 4.1) for $\{x_l(k)\}$ and $\{B(k)\}$ jointly, pooling across all $L$ trajectories. Store $\mathbf{X}^{(0)}$, $\mathbf{B}^{(0)}$. Set $A^{(0)}(k) = I$ for all $k$.

Main loop (iteration $t = 0, 1, 2, \ldots$): 2. COSMIC step: Using the current state estimates $\mathbf{X}^{(t)}$, solve the standard COSMIC problem (Section 4.2) pooling across trajectories. Obtain $\mathbf{C}^{(t+1)} = \{A^{(t+1)}(k), B^{(t+1)}(k)\}$. 3. State step: Using $\mathbf{C}^{(t+1)}$ (with trust-region interpolation $\tilde{A}(k) = (1-\mu)\,A^{(t+1)}(k) + \mu\,I$ if enabled), run the RTS smoother for each trajectory independently. Obtain $\mathbf{X}^{(t+1)}$. 4. Evaluate $J^{(t+1)} = J(\mathbf{X}^{(t+1)}, \mathbf{C}^{(t+1)})$. 5. If $|J^{(t+1)} - J^{(t)}| / |J^{(t)}| < \varepsilon_J$, declare convergence for the current $\mu$.

Trust-region outer loop (if enabled): 6. If $\mu > \varepsilon_\mu$: set $\mu \leftarrow \mu / 2$, warm-start from current iterates, return to step 2. 7. If $\mu \leq \varepsilon_\mu$: set $\mu = 0$, run steps 2–5 one final time.

When $H = I$: Take $R \to 0$ (or equivalently fix $x_l(k) = y_l(k)$). The state step is trivial, the initialisation is unnecessary, and the algorithm reduces to a single COSMIC solve — the original algorithm.

8. Computational Complexity¶

Initialisation: Single forward-backward pass with composite blocks, $O(N\,(Ln + nq)^3)$. For large $L$, exploitable structure reduces to $O(N\,(n^3 L + (nq)^3))$.
State step: RTS smoother, $O(N\,n^3)$ per trajectory, $O(L\,N\,n^3)$ total.
COSMIC step: Standard COSMIC tridiagonal solve, $O(N\,(n+q)^3)$, independent of $L$ (trajectories are pooled into the data matrices).
Per outer iteration: $O(L\,N\,n^3 + N\,(n+q)^3)$.
Total: Proportional to the number of inner iterations times the number of trust-region steps. In practice, convergence is expected within a small number of alternating iterations (typically 5–20), and trust-region steps (typically 0–5 if enabled).

The linear scaling in $N$ — the hallmark of COSMIC — is preserved.

9. Hyperparameter Selection¶

$\lambda$ (dynamics smoothness): Same role and selection criteria as in standard COSMIC. Controls the trade-off between data fidelity and temporal smoothness of the estimated system matrices.

$R$ (measurement noise covariance): The measurement noise covariance matrix, weighting the observation fidelity term via $R^{-1}$. When $R$ is known from sensor specifications or calibration, it is used directly — no tuning is required for the observation term. When $R$ is unknown, setting $R = I$ recovers unweighted least squares. The relative scaling between $R^{-1}$ and the dynamics fidelity term (which implicitly assumes unit process noise covariance) determines the balance between trusting measurements and trusting the dynamics model.

$\mu$ (trust-region): Start at $\mu = 1$ if used, halve adaptively. For well-conditioned problems, $\mu = 0$ from iteration 2 onward (i.e., trust-region disabled) is expected to suffice.

10. Relationship to Existing Methods¶

Expectation-Maximisation (EM) for LTV systems. The alternating scheme is structurally identical to EM for linear dynamical systems: the state step is the E-step (state smoothing given parameters), the COSMIC step is the M-step (parameter estimation given states). The key difference is that the M-step uses COSMIC's smoothness regularisation rather than maximum likelihood, making this a MAP-EM algorithm.

Subspace identification (N4SID, MOESP). These methods can provide alternative initialisations by estimating a local LTI model within sliding windows. The $A = I$ initialisation proposed here avoids the stationarity assumption inherent in windowed subspace methods and is better suited for systems with continuous parameter variation.

Kalman smoother. The state step is exactly an RTS smoother. Output-COSMIC can be viewed as wrapping a Kalman smoother with a COSMIC parameter estimator in an EM loop, with the COSMIC smoothness prior replacing the standard ML parameter update.

Appendix A. Forward-Backward Recursion for the State Step¶

A.1 Normal Equations¶

Given $A(k)$, $B(k)$ for $k = 0, \ldots, N-1$, observation matrix $H$, noise covariance $R$, measurements $y(k)$ for $k = 0, \ldots, N$, and inputs $u(k)$ for $k = 0, \ldots, N-1$, the state step minimises:

\[ \min_{\{x(k)\}} \sum_{k=0}^{N} \|y(k) - H\,x(k)\|^2_{R^{-1}} + \sum_{k=0}^{N-1} \|x(k+1) - A(k)\,x(k) - B(k)\,u(k)\|^2 \]

Setting $\partial J / \partial x(k) = 0$ for each $k$ yields a block tridiagonal system in $x(0), \ldots, x(N)$ with $n \times n$ blocks. Define $b(k) = B(k)\,u(k)$ for notational convenience.

Diagonal blocks:

\[ S_0 = H^\top R^{-1} H + A(0)^\top A(0) \]

\[ S_k = H^\top R^{-1} H + I + A(k)^\top A(k), \qquad k = 1, \ldots, N-1 \]

\[ S_N = H^\top R^{-1} H + I \]

Off-diagonal blocks (super-diagonal, coupling equation $k$ to unknown $x(k+1)$):

\[ U_k = -A(k)^\top, \qquad k = 0, \ldots, N-1 \]

By symmetry of the Hessian, the sub-diagonal block coupling equation $k$ to $x(k-1)$ is $L_k = U_{k-1}^\top = -A(k-1)$.

Right-hand side:

\[ \Theta_0 = H^\top R^{-1} y(0) - A(0)^\top b(0) \]

\[ \Theta_k = H^\top R^{-1} y(k) + b(k-1) - A(k)^\top b(k), \qquad k = 1, \ldots, N-1 \]

\[ \Theta_N = H^\top R^{-1} y(N) + b(N-1) \]

A.2 Forward-Backward Algorithm¶

The block tridiagonal system is solved by Gaussian elimination in the forward direction, followed by back-substitution. This is algebraically equivalent to the Rauch–Tung–Striebel smoother.

Forward pass ($k = 0$ to $N$):

\[ \Lambda_0 = S_0 \]

\[ Y_0 = \Lambda_0^{-1}\,\Theta_0 \]

For $k = 1, \ldots, N$:

\[ \Lambda_k = S_k - A(k-1)\,\Lambda_{k-1}^{-1}\,A(k-1)^\top \]

\[ Y_k = \Lambda_k^{-1}\bigl(\Theta_k + A(k-1)\,Y_{k-1}\bigr) \]

Backward pass ($k = N-1$ to $0$):

\[ x(N) = Y_N \]

For $k = N-1, \ldots, 0$:

\[ x(k) = Y_k + \Lambda_k^{-1}\,A(k)^\top\,x(k+1) \]

Complexity: $O(N\,n^3)$ per trajectory, $O(L\,N\,n^3)$ total. Each trajectory is independent.

A.3 Connection to COSMIC¶

This recursion is structurally identical to the COSMIC forward-backward pass (Section 4.2, COSMIC Step). The correspondence is:

COSMIC (dynamics estimation)	State step (state estimation)
Off-diagonal: $\lambda_{k+1}\,I$	Off-diagonal: $A(k)^\top$
Diagonal: $D(k)^\top D(k) + (\lambda_k + \lambda_{k+1})\,I$	Diagonal: $H^\top R^{-1} H + I + A(k)^\top A(k)$
Unknown: $C(k) \in \mathbb{R}^{(n+q) \times n}$	Unknown: $x(k) \in \mathbb{R}^n$
Backward: $C(k) = Y_k + \lambda_{k+1}\,\Lambda_k^{-1}\,C(k+1)$	Backward: $x(k) = Y_k + \Lambda_k^{-1}\,A(k)^\top\,x(k+1)$

The only structural difference is that the off-diagonal blocks in COSMIC are $\lambda_k\,I$ (scalar times identity), while in the state step they are $A(k)^\top$ (a general $n \times n$ matrix). The forward-backward pattern is identical.

A.4 Storage¶

Store $\Lambda_k^{-1}$ and $Y_k$ for $k = 0, \ldots, N$ during the forward pass (total: $(N+1)$ matrices of size $n \times n$ each, plus $(N+1)$ vectors of size $n$). These are reused in the backward pass. The same stored $\Lambda_k^{-1}$ can be reused across EM iterations if $A(k)$ has not changed significantly (warm-starting), though for correctness they must be recomputed whenever $C$ changes.

Appendix B. Initialisation Solve¶

B.1 Problem Structure¶

The initialisation minimises $J_{\text{init}} = J\big|_{A=I}$ jointly over $\{x_l(k)\}$ and $\{B(k)\}$. This is a single convex quadratic with a unique minimiser. Since $B(k)\,u_l(k)$ is linear in $B(k)$ (because $u_l(k)$ is known data) and $x_l(k)$ appears linearly, there is no bilinear coupling between the two sets of unknowns. The joint normal equations form a block tridiagonal system in time that is solved in a single forward-backward pass.

B.2 Composite Unknowns¶

Define the composite unknown at each time step:

\[ w(k) = \begin{bmatrix} x_1(k) \\ \vdots \\ x_L(k) \\ \text{vec}(B(k)) \end{bmatrix} \in \mathbb{R}^{Ln + nq}, \qquad k = 0, \ldots, N-1 \]

\[ w(N) = \begin{bmatrix} x_1(N) \\ \vdots \\ x_L(N) \end{bmatrix} \in \mathbb{R}^{Ln} \]

The normal equations $\partial J_{\text{init}} / \partial w(k) = 0$ form a block tridiagonal system in $w(0), \ldots, w(N)$.

B.3 Block Definitions¶

For each trajectory $l$, define $e_l(k) = u_l(k)^\top \otimes I_n \in \mathbb{R}^{n \times nq}$, which maps $\text{vec}(B(k))$ to $B(k)\,u_l(k)$. Stack these: $E(k) = [e_1(k);\; \ldots;\; e_L(k)] \in \mathbb{R}^{Ln \times nq}$.

Diagonal blocks (interior $0 < k < N$):

\[ S_k = \begin{bmatrix} I_L \otimes (H^\top R^{-1} H + 2I_n) & E(k) - E(k-1) \\ (E(k) - E(k-1))^\top & P(k) + 2\lambda\,I_{nq} \end{bmatrix} \]

where $P(k) = \sum_{l=1}^{L} (u_l(k)\,u_l(k)^\top) \otimes I_n \in \mathbb{R}^{nq \times nq}$ is the pooled input Gram matrix.

The $x$–$B$ coupling block $E(k) - E(k-1)$ arises because the equation for $x_l(k)$ involves $+B(k)\,u_l(k) - B(k-1)\,u_l(k-1)$: the current $B(k)$ from the dynamics residual at $k$, and the previous $B(k-1)$ from the dynamics residual at $k-1$.

Boundary diagonal blocks:

\[ S_0 = \begin{bmatrix} I_L \otimes (H^\top R^{-1} H + I_n) & E(0) \\ E(0)^\top & P(0) + \lambda\,I_{nq} \end{bmatrix} \]

\[ S_N = I_L \otimes (H^\top R^{-1} H + I_n) \qquad \text{(no $B$ block at $k = N$)} \]

Off-diagonal blocks (super-diagonal, coupling $w(k)$ to $w(k+1)$, for $0 \leq k \leq N-2$):

\[ U_k = \begin{bmatrix} -I_L \otimes I_n & 0 \\ -E(k)^\top & -\lambda\,I_{nq} \end{bmatrix} \]

The top-left block $-I_L \otimes I_n$ is the $x(k) \to x(k+1)$ coupling through the dynamics (with $A = I$). The bottom-left block $-E(k)^\top$ is the $B(k) \to x(k+1)$ coupling. The bottom-right block $-\lambda\,I_{nq}$ is the smoothness coupling $B(k) \to B(k+1)$.

For $k = N-1$: $U_{N-1} = \begin{bmatrix} -I_L \otimes I_n \\ -E(N-1)^\top \end{bmatrix}$ (no $B$ column at $k = N$).

Right-hand side:

\[ \Theta_0 = \begin{bmatrix} [H^\top R^{-1} y_1(0);\; \ldots;\; H^\top R^{-1} y_L(0)] \\ 0_{nq} \end{bmatrix} \]

\[ \Theta_k = \begin{bmatrix} [H^\top R^{-1} y_1(k);\; \ldots;\; H^\top R^{-1} y_L(k)] \\ 0_{nq} \end{bmatrix}, \qquad 0 < k < N \]

\[ \Theta_N = [H^\top R^{-1} y_1(N);\; \ldots;\; H^\top R^{-1} y_L(N)] \]

B.4 Forward-Backward Algorithm¶

The same forward-backward algorithm as Appendix A applies with the composite blocks:

Forward pass ($k = 0$ to $N$):

\[ \Lambda_0 = S_0, \qquad Y_0 = \Lambda_0^{-1}\,\Theta_0 \]

For $k = 1, \ldots, N$:

\[ \Lambda_k = S_k - U_{k-1}^\top\,\Lambda_{k-1}^{-1}\,U_{k-1} \]

\[ Y_k = \Lambda_k^{-1}\bigl(\Theta_k + U_{k-1}^\top\,Y_{k-1}\bigr) \]

Note: $L_k = U_{k-1}^\top$ by symmetry of the Hessian. The sign convention matches: $U_{k-1}^\top\,\Lambda_{k-1}^{-1}\,U_{k-1}$ is the Schur complement elimination of $w(k-1)$ from the equation for $w(k)$, and $U_{k-1}^\top\,Y_{k-1}$ propagates the eliminated RHS forward.

Backward pass ($k = N-1$ to $0$):

The forward elimination produces the reduced system $\Lambda_k\,w(k) + U_k\,w(k+1) = \tilde{\Theta}_k$, where $Y_k = \Lambda_k^{-1}\,\tilde{\Theta}_k$. Solving for $w(k)$:

\[ w(N) = Y_N \]

For $k = N-1, \ldots, 0$:

\[ w(k) = Y_k - \Lambda_k^{-1}\,U_k\,w(k+1) \]

Since $U_k$ contains negative entries (e.g., $-I_L \otimes I_n$ in the top-left), the subtraction of a negative term produces a positive contribution, matching the sign convention of the COSMIC backward pass.

After the backward pass, extract $x_l(k)$ and $B(k)$ from each $w(k)$.

B.5 Complexity¶

Each forward or backward step involves operations on blocks of size $(Ln + nq)$. The dominant cost is the matrix inversion $\Lambda_k^{-1}$ at each step: $O((Ln + nq)^3)$. Total: $O(N\,(Ln + nq)^3)$.

For typical problem sizes ($L \leq 10$, $n \leq 10$, $q \leq 5$), the block dimension is at most $\sim 150$, and the cubic cost is negligible. For very large $L$, the block-diagonal-plus-low-rank structure of $S_k$ (the $x$-blocks are independent across trajectories, coupled only through $B$) can be exploited via the Woodbury identity to reduce cost to $O(N\,(n^3 L + (nq)^3))$.

B.6 Transition to Main Loop¶

After the forward-backward pass, the estimated $\{x_l(k)\}$ and $\{B(k)\}$ are stored as $\mathbf{X}^{(0)}$ and $\mathbf{B}^{(0)}$, with $A^{(0)}(k) = I$ for all $k$. The main alternating loop (Section 4.2) begins with the COSMIC step, which estimates $A(k)$ and refines $B(k)$ using the full $C(k) = [A^\top(k);\; B^\top(k)]$ parameterisation.

COSMIC (dynamics estimation)	State step (state estimation)
Off-diagonal: \(\lambda_{k+1}\,I\)	Off-diagonal: \(A(k)^\top\)
Diagonal: \(D(k)^\top D(k) + (\lambda_k + \lambda_{k+1})\,I\)	Diagonal: \(H^\top R^{-1} H + I + A(k)^\top A(k)\)
Unknown: \(C(k) \in \mathbb{R}^{(n+q) \times n}\)	Unknown: \(x(k) \in \mathbb{R}^n\)
Backward: \(C(k) = Y_k + \lambda_{k+1}\,\Lambda_k^{-1}\,C(k+1)\)	Backward: \(x(k) = Y_k + \Lambda_k^{-1}\,A(k)^\top\,x(k+1)\)