Skip to content

Sensitivity Matrix

The sensitivity matrix extends variational equations to include the effect of uncertain parameters on the state. While the State Transition Matrix (STM) maps initial state uncertainties to final state uncertainties, the sensitivity matrix maps parameter uncertainties to state uncertainties.

What is the Sensitivity Matrix?

For a dynamical system that depends on both state and parameters:

\[\dot{\mathbf{x}} = \mathbf{f}(t, \mathbf{x}, \mathbf{p})\]

where \(\mathbf{p}\) is a vector of "consider parameters" (parameters that affect dynamics but aren't estimated), the sensitivity matrix \(\mathbf{S}\) describes how state evolves with respect to parameter changes:

\[\mathbf{S}(t) = \frac{\partial \mathbf{x}(t)}{\partial \mathbf{p}}\]

The sensitivity matrix satisfies the differential equation:

\[\dot{\mathbf{S}} = \frac{\partial \mathbf{f}(t, \mathbf{x}, \mathbf{p})}{\partial \mathbf{x}} \mathbf{S} + \frac{\partial \mathbf{f}(t, \mathbf{x}, \mathbf{p})}{\partial \mathbf{p}}\]

Sensitivity matrices are essential for accounting for parameter uncertainties in orbit determination.

Relationship to STM

The sensitivity matrix and STM serve related but distinct purposes:

Matrix Equation Maps
STM \(\Phi\) \(\dot{\Phi} = \mathbf{A}\Phi\) Initial state → Final state
Sensitivity \(\mathbf{S}\) \(\dot{\mathbf{S}} = \mathbf{A}\mathbf{S} + \mathbf{B}\) Parameters → Final state

Propagating the Sensitivity Matrix

Brahe integrators provide step_with_sensmat() for propagating the sensitivity matrix alongside the state:

1
2
3
// Result: (new_state, new_sensitivity, dt_used, error, dt_next)
let (state, sens, dt_used, error, dt_next) =
    integrator.step_with_sensmat(t, state, sensitivity, &params, dt);

For combined STM and sensitivity propagation:

1
2
3
// Result: (new_state, new_stm, new_sensitivity, dt_used, error, dt_next)
let (state, phi, sens, dt_used, error, dt_next) =
    integrator.step_with_varmat_sensmat(t, state, phi, sensitivity, &params, dt);

Using Sensitivity Providers

Brahe provides two approaches for computing the sensitivity matrix \(\partial \mathbf{f}/\partial \mathbf{p}\)---NumericalSensitivity and AnalyticSensitivity classes. The NumericalSensitivity provider computes sensitivities automatically by perturbing parameters, while AnalyticSensitivity allows you to supply analytical derivatives for better performance. When you know the analytical form of \(\partial \mathbf{f}/\partial \mathbf{p}\), use AnalyticSensitivity for better accuracy and performance. The example files below demonstrate both numerical and analytical approaches.

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
import brahe as bh
import numpy as np


def dynamics_with_params(t, state, params):
    """Orbital dynamics with consider parameters.

    Args:
        t: Time
        state: [x, y, z, vx, vy, vz]
        params: [cd_area_m] - drag coefficient * area / mass
    """
    # Extract parameter
    cd_area_m = params[0]

    # Gravitational dynamics
    r = state[:3]
    v = state[3:]
    r_norm = np.linalg.norm(r)
    a_grav = -bh.GM_EARTH / (r_norm**3) * r

    # Atmospheric drag (simplified exponential model)
    h = r_norm - bh.R_EARTH
    rho0 = 1.225  # kg/m^3 at sea level
    H = 8500.0  # Scale height in meters
    rho = rho0 * np.exp(-h / H)

    v_norm = np.linalg.norm(v)
    a_drag = -0.5 * rho * cd_area_m * v_norm * v

    return np.concatenate([v, a_grav + a_drag])


def analytical_sensitivity(t, state, params):
    """Analytical sensitivity ∂f/∂p for drag parameter.

    Args:
        t: Time
        state: [x, y, z, vx, vy, vz]
        params: [cd_area_m]

    Returns:
        6x1 sensitivity matrix
    """
    r = state[:3]
    v = state[3:]
    r_norm = np.linalg.norm(r)

    # Atmospheric density
    h = r_norm - bh.R_EARTH
    rho0 = 1.225
    H = 8500.0
    rho = rho0 * np.exp(-h / H)

    v_norm = np.linalg.norm(v)

    # ∂(state_dot)/∂(cd_area_m)
    sens = np.zeros((6, 1))
    if v_norm > 0:
        # ∂(a_drag)/∂(cd_area_m) = -0.5 * rho * v_norm * v
        sens[3:6, 0] = -0.5 * rho * v_norm * v

    return sens


# Initial state (400 km LEO circular orbit)
oe = np.array([bh.R_EARTH + 250e3, 0.001, 51.6, 0.0, 0.0, 0.0])
state = bh.state_koe_to_eci(oe, bh.AngleFormat.DEGREES)

# Consider parameters
params = np.array([0.044])  # cd_area_m = Cd*A/m = 2.2*10/500

# Create numerical sensitivity provider (use class directly as constructor)
numerical_sens = bh.NumericalSensitivity(dynamics_with_params)

# Compute sensitivity matrix numerically
sens_numerical = numerical_sens.compute(0.0, state, params)

print("Numerical sensitivity (∂f/∂p):")
print(
    f"  Position rates: [{sens_numerical[0, 0]}, {sens_numerical[1, 0]}, {sens_numerical[2, 0]}]"
)
print(
    f"  Velocity rates: [{sens_numerical[3, 0]}, {sens_numerical[4, 0]}, {sens_numerical[5, 0]}]"
)

# Create analytical sensitivity provider
analytic_sens = bh.AnalyticSensitivity(analytical_sensitivity)

# Compute sensitivity matrix analytically
sens_analytical = analytic_sens.compute(0.0, state, params)

print("\nAnalytical sensitivity (∂f/∂p):")
print(
    f"  Position rates: [{sens_analytical[0, 0]}, {sens_analytical[1, 0]}, {sens_analytical[2, 0]}]"
)
print(
    f"  Velocity rates: [{sens_analytical[3, 0]}, {sens_analytical[4, 0]}, {sens_analytical[5, 0]}]"
)

# Compare numerical and analytical
diff = np.abs(sens_numerical - sens_analytical)
print(f"\nMax difference: {np.max(diff):.3e}")

# Numerical sensitivity (∂f/∂p):
#   Position rates: [0, 0, 0]
#   Velocity rates: [0, -0.000008425648220011794, -0.000010630522385923769]

# Analytical sensitivity (∂f/∂p):
#   Position rates: [0, 0, 0]
#   Velocity rates: [0, -0.000008425648218908737, -0.00001063052238539645]

# Max difference: 1.103e-15

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
use brahe::constants::{GM_EARTH, R_EARTH};
use brahe::math::sensitivity::{
    DAnalyticSensitivity, DNumericalSensitivity, DSensitivityProvider,
};
use brahe::state_koe_to_eci;
use brahe::AngleFormat;
use nalgebra::{DMatrix, DVector, SVector};

fn main() {
    // Dynamics function that takes consider parameters
    let dynamics = |_t: f64, state: &DVector<f64>, params: &DVector<f64>| -> DVector<f64> {
        let cd_area_m = params[0];

        let r = state.fixed_rows::<3>(0);
        let v = state.fixed_rows::<3>(3);
        let r_norm = r.norm();

        // Gravitational acceleration
        let a_grav = -GM_EARTH / (r_norm * r_norm * r_norm) * r;

        // Atmospheric drag (simplified exponential model)
        let h = r_norm - R_EARTH;
        let rho0 = 1.225; // kg/m^3
        let scale_height = 8500.0; // m
        let rho = rho0 * (-h / scale_height).exp();

        let v_norm = v.norm();
        let a_drag = if v_norm > 0.0 {
            -0.5 * rho * cd_area_m * v_norm * v
        } else {
            nalgebra::Vector3::zeros()
        };

        let mut state_dot = DVector::<f64>::zeros(6);
        state_dot
            .fixed_rows_mut::<3>(0)
            .copy_from(&v.clone_owned());
        state_dot
            .fixed_rows_mut::<3>(3)
            .copy_from(&(a_grav + a_drag));
        state_dot
    };

    // Analytical sensitivity function
    let analytical_sensitivity =
        |_t: f64, state: &DVector<f64>, _params: &DVector<f64>| -> DMatrix<f64> {
            let r = state.fixed_rows::<3>(0);
            let v = state.fixed_rows::<3>(3);
            let r_norm = r.norm();

            // Atmospheric density
            let h = r_norm - R_EARTH;
            let rho0 = 1.225;
            let scale_height = 8500.0;
            let rho = rho0 * (-h / scale_height).exp();

            let v_norm = v.norm();

            // ∂(state_dot)/∂(cd_area_m)
            let mut sens = DMatrix::<f64>::zeros(6, 1);
            if v_norm > 0.0 {
                // ∂(a_drag)/∂(cd_area_m) = -0.5 * rho * v_norm * v
                for i in 0..3 {
                    sens[(i + 3, 0)] = -0.5 * rho * v_norm * v[i];
                }
            }

            sens
        };

    // Initial state (400 km LEO circular orbit)
    let oe = SVector::<f64, 6>::from_row_slice(&[R_EARTH + 250e3, 0.001, 51.6, 0.0, 0.0, 0.0]);
    let state_vec = state_koe_to_eci(oe, AngleFormat::Degrees);
    let state = DVector::from_iterator(6, state_vec.iter().copied());

    // Consider parameters
    let params = DVector::from_vec(vec![0.044]); // cd_area_m = Cd*A/m = 2.2*10/500

    // Create numerical sensitivity provider
    let numerical_sens = DNumericalSensitivity::central(Box::new(dynamics.clone()));

    // Compute sensitivity matrix numerically
    let sens_numerical = numerical_sens.compute(0.0, &state, &params);

    println!("Numerical sensitivity (∂f/∂p):");
    println!(
        "  Position rates: [{}, {}, {}]",
        sens_numerical[(0, 0)],
        sens_numerical[(1, 0)],
        sens_numerical[(2, 0)]
    );
    println!(
        "  Velocity rates: [{}, {}, {}]",
        sens_numerical[(3, 0)],
        sens_numerical[(4, 0)],
        sens_numerical[(5, 0)]
    );

    // Create analytical sensitivity provider
    let analytic_sens = DAnalyticSensitivity::new(Box::new(analytical_sensitivity));

    // Compute sensitivity matrix analytically
    let sens_analytical = analytic_sens.compute(0.0, &state, &params);

    println!("\nAnalytical sensitivity (∂f/∂p):");
    println!(
        "  Position rates: [{}, {}, {}]",
        sens_analytical[(0, 0)],
        sens_analytical[(1, 0)],
        sens_analytical[(2, 0)]
    );
    println!(
        "  Velocity rates: [{}, {}, {}]",
        sens_analytical[(3, 0)],
        sens_analytical[(4, 0)],
        sens_analytical[(5, 0)]
    );

    // Compare numerical and analytical
    let diff = &sens_numerical - &sens_analytical;
    let max_diff = diff.abs().max();
    println!("\nMax difference: {:.3e}", max_diff);
}

// Numerical sensitivity (∂f/∂p):
//   Position rates: [0, 0, 0]
//   Velocity rates: [0, -0.000008425648220011794, -0.000010630522385923769]

// Analytical sensitivity (∂f/∂p):
//   Position rates: [0, 0, 0]
//   Velocity rates: [0, -0.000008425648218908737, -0.00001063052238539645]

// Max difference: 1.103e-15

When to Use Analytical Sensitivity

Use analytical sensitivity when:

  • Derivatives \(\partial \mathbf{f}/\partial \mathbf{p}\) are simple to derive (e.g., drag coefficient, solar radiation pressure coefficient)
  • Maximum accuracy is required (no finite difference errors)
  • Sensitivity will be computed many times (performance critical)
  • Working with well-understood parameter dependencies

Recommendation

For common parameters like atmospheric drag coefficient or solar radiation pressure coefficient, the analytical derivatives are often straightforward. When analytical forms are available, they provide better accuracy and performance than numerical approximations.

Perturbation Strategies

The NumericalSensitivity provider uses the same perturbation strategies as NumericalJacobian:

  • Fixed perturbation: Constant step size for all parameters
  • Percentage perturbation: Scale by parameter magnitude
  • Adaptive perturbation: Balance accuracy and robustness

See the Jacobian Computation guide for detailed information on configuring perturbation strategies.

Integrating Sensitivity Matrices

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
"""

import brahe as bh
import numpy as np


def dynamics_with_params(t, state, params):
    """Orbital dynamics with atmospheric drag.

    Args:
        t: Time
        state: [x, y, z, vx, vy, vz]
        params: [cd_area_m] - drag coefficient * area / mass
    """
    cd_area_m = params[0]

    r = state[:3]
    v = state[3:]
    r_norm = np.linalg.norm(r)
    a_grav = -bh.GM_EARTH / (r_norm**3) * r

    # Atmospheric drag (simplified exponential model)
    h = r_norm - bh.R_EARTH
    rho0 = 1.225  # kg/m^3 at sea level
    H = 8500.0  # Scale height in meters
    rho = rho0 * np.exp(-h / H)

    v_norm = np.linalg.norm(v)
    a_drag = -0.5 * rho * cd_area_m * v_norm * v

    return np.concatenate([v, a_grav + a_drag])


# Consider parameters
cd_area_m = 2.2 * 10.0 / 500.0  # Cd=2.2, A=10m^2, m=500kg
params = np.array([cd_area_m])
num_params = len(params)

# Create sensitivity provider using NumericalSensitivity
sensitivity_provider = bh.NumericalSensitivity.central(dynamics_with_params)

# Create Jacobian provider using NumericalJacobian
jacobian_provider = bh.NumericalJacobian.central(
    lambda t, s: dynamics_with_params(t, s, params)
)


def augmented_dynamics(t, aug_state):
    """Augmented dynamics for state + sensitivity matrix propagation.

    Propagates:
        dx/dt = f(t, x, p)
        dΦ/dt = (∂f/∂x) * Φ + (∂f/∂p)

    Args:
        t: Time
        aug_state: [state (6), vec(Φ) (6*num_params)]
    """
    state = aug_state[:6]
    phi = aug_state[6:].reshape(6, num_params)

    # State derivative
    state_dot = dynamics_with_params(t, state, params)

    # Compute Jacobian ∂f/∂x
    jacobian = jacobian_provider.compute(t, state)

    # Compute sensitivity ∂f/∂p
    sensitivity = sensitivity_provider.compute(t, state, params)

    # Sensitivity matrix derivative: dΦ/dt = J*Φ + S
    phi_dot = jacobian @ phi + sensitivity

    return np.concatenate([state_dot, phi_dot.flatten()])


# Initial state (200 km LEO for significant drag effects)
oe = np.array([bh.R_EARTH + 200e3, 0.001, 51.6, 0.0, 0.0, 0.0])
state = bh.state_koe_to_eci(oe, bh.AngleFormat.DEGREES)

# Initial sensitivity matrix (identity would mean we start with unit sensitivity,
# but we start with zero since we're interested in how sensitivity develops)
phi0 = np.zeros((6, num_params))

# Augmented initial state
aug_state = np.concatenate([state, phi0.flatten()])

# Create integrator for augmented system
# Using fixed step RK4 for simplicity and exact parity with Rust
aug_dim = 6 + 6 * num_params
config = bh.IntegratorConfig.fixed_step(1.0)
integrator = bh.RK4Integrator(aug_dim, augmented_dynamics, config=config)

# Propagate for 1 hour
t = 0.0
dt = 1.0
t_final = 3600.0

while t < t_final:
    aug_state = integrator.step(t, aug_state, dt)
    t += dt

# Extract final state and sensitivity matrix
final_state = aug_state[:6]
final_phi = aug_state[6:].reshape(6, num_params)

print(f"Final position after {t_final / 60:.0f} minutes:")
print(f"  x: {final_state[0] / 1000:.3f} km")
print(f"  y: {final_state[1] / 1000:.3f} km")
print(f"  z: {final_state[2] / 1000:.3f} km")

print("\nSensitivity matrix Φ = ∂x/∂p (position per unit Cd*A/m):")
print(f"  dx/dp: {final_phi[0, 0]:.3f} m/(m²/kg)")
print(f"  dy/dp: {final_phi[1, 0]:.3f} m/(m²/kg)")
print(f"  dz/dp: {final_phi[2, 0]:.3f} m/(m²/kg)")

print("\nSensitivity matrix Φ = ∂x/∂p (velocity per unit Cd*A/m):")
print(f"  dvx/dp: {final_phi[3, 0]:.6f} m/s/(m²/kg)")
print(f"  dvy/dp: {final_phi[4, 0]:.6f} m/s/(m²/kg)")
print(f"  dvz/dp: {final_phi[5, 0]:.6f} m/s/(m²/kg)")

# Interpret: If we have 10% uncertainty in Cd*A/m (0.1 * 0.044 = 0.0044),
# the position uncertainty after 1 hour would be:
delta_p = 0.1 * cd_area_m
pos_uncertainty = np.linalg.norm(final_phi[:3, 0]) * delta_p
print(f"\nPosition uncertainty for 10% parameter uncertainty: {pos_uncertainty:.1f} m")

# Expected output:
# Final position after 60 minutes:
#   x: -2884.245 km
#   y: -3673.659 km
#   z: -4635.004 km

# Sensitivity matrix Φ = ∂x/∂p (position per unit Cd*A/m):
#   dx/dp: 59942.894 m/(m²/kg)
#   dy/dp: -3796.878 m/(m²/kg)
#   dz/dp: -4790.467 m/(m²/kg)

# Sensitivity matrix Φ = ∂x/∂p (velocity per unit Cd*A/m):
#   dvx/dp: 44.091413 m/s/(m²/kg)
#   dvy/dp: 33.444231 m/s/(m²/kg)
#   dvz/dp: 42.196119 m/s/(m²/kg)

# Position uncertainty for 10% parameter uncertainty: 265.1 m

  1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
//! Sensitivity matrix propagation pattern example.
//!
//! Demonstrates propagating the sensitivity matrix alongside state to map
//! parameter uncertainties to state uncertainties over time.
//!
//! The sensitivity matrix Φ = ∂x/∂p evolves according to:
//!     dΦ/dt = (∂f/∂x) * Φ + (∂f/∂p)
//!
//! This augmented state approach propagates [state, vec(Φ)] together.

use brahe::constants::{GM_EARTH, R_EARTH};
use brahe::integrators::*;
use brahe::math::jacobian::{DJacobianProvider, DNumericalJacobian};
use brahe::math::sensitivity::{DNumericalSensitivity, DSensitivityProvider};
use brahe::{state_koe_to_eci, AngleFormat};
use nalgebra::{DVector, SVector};

fn main() {
    // Consider parameters
    let cd_area_m = 2.2 * 10.0 / 500.0; // Cd=2.2, A=10m^2, m=500kg
    let params = DVector::from_vec(vec![cd_area_m]);
    let num_params = params.len();

    // Dynamics function with parameters
    let dynamics_with_params =
        |_t: f64, state: &DVector<f64>, params: &DVector<f64>| -> DVector<f64> {
            let cd_area_m = params[0];

            let r = state.fixed_rows::<3>(0);
            let v = state.fixed_rows::<3>(3);
            let r_norm = r.norm();

            // Gravitational acceleration
            let a_grav = -GM_EARTH / (r_norm * r_norm * r_norm) * r;

            // Atmospheric drag (simplified exponential model)
            let h = r_norm - R_EARTH;
            let rho0 = 1.225; // kg/m^3
            let scale_height = 8500.0; // m
            let rho = rho0 * (-h / scale_height).exp();

            let v_norm = v.norm();
            let a_drag = if v_norm > 0.0 {
                -0.5 * rho * cd_area_m * v_norm * v
            } else {
                nalgebra::Vector3::zeros()
            };

            let mut state_dot = DVector::<f64>::zeros(6);
            state_dot
                .fixed_rows_mut::<3>(0)
                .copy_from(&v.clone_owned());
            state_dot
                .fixed_rows_mut::<3>(3)
                .copy_from(&(a_grav + a_drag));
            state_dot
        };

    // Create sensitivity provider
    let sensitivity_provider =
        DNumericalSensitivity::central(Box::new(dynamics_with_params.clone()));

    // Create Jacobian provider (dynamics without explicit params for Jacobian)
    let params_clone = params.clone();
    let dynamics_for_jacobian = move |_t: f64, state: &DVector<f64>, _params: Option<&DVector<f64>>| -> DVector<f64> {
        let cd_area_m = params_clone[0];

        let r = state.fixed_rows::<3>(0);
        let v = state.fixed_rows::<3>(3);
        let r_norm = r.norm();

        let a_grav = -GM_EARTH / (r_norm * r_norm * r_norm) * r;

        let h = r_norm - R_EARTH;
        let rho0 = 1.225;
        let scale_height = 8500.0;
        let rho = rho0 * (-h / scale_height).exp();

        let v_norm = v.norm();
        let a_drag = if v_norm > 0.0 {
            -0.5 * rho * cd_area_m * v_norm * v
        } else {
            nalgebra::Vector3::zeros()
        };

        let mut state_dot = DVector::<f64>::zeros(6);
        state_dot
            .fixed_rows_mut::<3>(0)
            .copy_from(&v.clone_owned());
        state_dot
            .fixed_rows_mut::<3>(3)
            .copy_from(&(a_grav + a_drag));
        state_dot
    };

    let jacobian_provider = DNumericalJacobian::central(Box::new(dynamics_for_jacobian));

    // Augmented dynamics for state + sensitivity matrix propagation
    let params_for_aug = params.clone();
    let augmented_dynamics = move |t: f64,
                                   aug_state: &DVector<f64>,
                                   _params: Option<&DVector<f64>>|
          -> DVector<f64> {
        // Extract state and sensitivity matrix
        let state = aug_state.rows(0, 6).into_owned();
        let phi_flat = aug_state.rows(6, 6 * num_params).into_owned();

        // Reshape phi from flat vector to matrix (column-major)
        let mut phi = nalgebra::DMatrix::<f64>::zeros(6, num_params);
        for j in 0..num_params {
            for i in 0..6 {
                phi[(i, j)] = phi_flat[j * 6 + i];
            }
        }

        // State derivative
        let state_dot = dynamics_with_params(t, &state, &params_for_aug);

        // Compute Jacobian ∂f/∂x
        let jacobian = jacobian_provider.compute(t, &state, None);

        // Compute sensitivity ∂f/∂p
        let sensitivity = sensitivity_provider.compute(t, &state, &params_for_aug);

        // Sensitivity matrix derivative: dΦ/dt = J*Φ + S
        let phi_dot = &jacobian * &phi + &sensitivity;

        // Flatten phi_dot back to vector (column-major)
        let mut phi_dot_flat = DVector::<f64>::zeros(6 * num_params);
        for j in 0..num_params {
            for i in 0..6 {
                phi_dot_flat[j * 6 + i] = phi_dot[(i, j)];
            }
        }

        // Concatenate state_dot and phi_dot
        let mut aug_dot = DVector::<f64>::zeros(6 + 6 * num_params);
        aug_dot.rows_mut(0, 6).copy_from(&state_dot);
        aug_dot.rows_mut(6, 6 * num_params).copy_from(&phi_dot_flat);
        aug_dot
    };

    // Initial state (200 km LEO for significant drag effects)
    let oe = SVector::<f64, 6>::from_row_slice(&[R_EARTH + 200e3, 0.001, 51.6, 0.0, 0.0, 0.0]);
    let state_vec = state_koe_to_eci(oe, AngleFormat::Degrees);
    let state = DVector::from_iterator(6, state_vec.iter().copied());

    // Initial sensitivity matrix (zeros - we're interested in how it develops)
    let phi0 = DVector::<f64>::zeros(6 * num_params);

    // Augmented initial state
    let mut aug_state = DVector::<f64>::zeros(6 + 6 * num_params);
    aug_state.rows_mut(0, 6).copy_from(&state);
    aug_state.rows_mut(6, 6 * num_params).copy_from(&phi0);

    // Create integrator for augmented system
    // Using fixed step RK4 for simplicity and exact parity with Python
    let aug_dim = 6 + 6 * num_params;
    let config = IntegratorConfig::fixed_step(1.0);
    let integrator = RK4DIntegrator::with_config(
        aug_dim,
        Box::new(augmented_dynamics),
        None,
        None,
        None,
        config,
    );

    // Propagate for 1 hour
    let t_final = 3600.0;
    let mut t = 0.0;
    let dt = 1.0;

    while t < t_final {
        aug_state = integrator.step(t, aug_state, None, None).state;
        t += dt;
    }

    // Extract final state and sensitivity matrix
    let final_state = aug_state.rows(0, 6).into_owned();
    let final_phi_flat = aug_state.rows(6, 6 * num_params).into_owned();

    // Reshape to matrix
    let mut final_phi = nalgebra::DMatrix::<f64>::zeros(6, num_params);
    for j in 0..num_params {
        for i in 0..6 {
            final_phi[(i, j)] = final_phi_flat[j * 6 + i];
        }
    }

    println!("Final position after {} minutes:", t_final / 60.0);
    println!("  x: {:.3} km", final_state[0] / 1000.0);
    println!("  y: {:.3} km", final_state[1] / 1000.0);
    println!("  z: {:.3} km", final_state[2] / 1000.0);

    println!("\nSensitivity matrix Φ = ∂x/∂p (position per unit Cd*A/m):");
    println!("  dx/dp: {:.3} m/(m²/kg)", final_phi[(0, 0)]);
    println!("  dy/dp: {:.3} m/(m²/kg)", final_phi[(1, 0)]);
    println!("  dz/dp: {:.3} m/(m²/kg)", final_phi[(2, 0)]);

    println!("\nSensitivity matrix Φ = ∂x/∂p (velocity per unit Cd*A/m):");
    println!("  dvx/dp: {:.6} m/s/(m²/kg)", final_phi[(3, 0)]);
    println!("  dvy/dp: {:.6} m/s/(m²/kg)", final_phi[(4, 0)]);
    println!("  dvz/dp: {:.6} m/s/(m²/kg)", final_phi[(5, 0)]);

    // Interpret: If we have 10% uncertainty in Cd*A/m,
    // the position uncertainty after 1 hour would be:
    let delta_p = 0.1 * cd_area_m;
    let pos_uncertainty = (final_phi[(0, 0)].powi(2)
        + final_phi[(1, 0)].powi(2)
        + final_phi[(2, 0)].powi(2))
    .sqrt()
        * delta_p;
    println!(
        "\nPosition uncertainty for 10% parameter uncertainty: {:.1} m",
        pos_uncertainty
    );
}

// Example output:
// Final position after 60 minutes:
//   x: -2884.245 km
//   y: -3673.659 km
//   z: -4635.004 km

// Sensitivity matrix Φ = ∂x/∂p (position per unit Cd*A/m):
//   dx/dp: 59942.895 m/(m²/kg)
//   dy/dp: -3796.877 m/(m²/kg)
//   dz/dp: -4790.467 m/(m²/kg)

// Sensitivity matrix Φ = ∂x/∂p (velocity per unit Cd*A/m):
//   dvx/dp: 44.091415 m/s/(m²/kg)
//   dvy/dp: 33.444232 m/s/(m²/kg)
//   dvz/dp: 42.196119 m/s/(m²/kg)

// Position uncertainty for 10% parameter uncertainty: 265.1 m

See Also