Math Module API

var pid = new PidController(kp: 2.0, ki: 0.5, kd: 0.1)
{
    OutputMin = -127,
    OutputMax = 127,
    
    // Anti-windup protection
    AntiWindupMode = AntiWindupMode.BackCalculation,
    IntegralWindupThreshold = 100,
    
    // Derivative filter (1st-order lowpass)
    DerivativeFilterTf = 0.01,
    
    // Setpoint weighting for smoother transitions
    SetpointWeight = 1.0,
    DerivativeWeighting = 0.0  // don't react to setpoint jumps
};

while (true)
{
    double output = pid.Compute(setpoint: 100, measurement: 75, dt: 0.01);
    motor.Power = (int)output;
    
    if (pid.IsWindupActive)
        logger.LogWarning("Integrator saturated");
}

// MPC configuration with constraints
var mpc = new ModelPredictiveController(A, B, Q, R, horizonLength: 20)
{
    // Constraints on states and inputs
    StateMin = new[] { -100, -100, -5, -5 },   // x, y, vx, vy
    StateMax = new[] { 100, 100, 5, 5 },
    
    InputMin = new[] { -2, -2 },               // ax, ay
    InputMax = new[] { 2, 2 },
    
    // Rate limits on inputs (prevent jerky commands)
    InputRateMin = new[] { -0.5, -0.5 },
    InputRateMax = new[] { 0.5, 0.5 },
    
    // Solver tolerance
    SolverTolerance = 1e-4,
    MaxIterations = 100
};

// Solve optimization problem
var control = mpc.ComputeControl(currentState, referenceTrajectory);
System.Diagnostics.Debug.WriteLine($"Solve time: {mpc.LastSolveTime}ms");

// Solve continuous-time LQR problem: min ∫(x'Qx + u'Ru)dt
var result = LqrDesign.SolveContinuous(A, B, Q, R);

if (result.Success)
{
    var K = result.K;          // Optimal feedback gain
    var P = result.P;          // Solution to Riccati equation
    var eigenvalues = result.EigenvaluesCL;  // Closed-loop poles
    
    // Apply control law: u = -K * (x - x_ref)
    var error = currentState - referenceState;
    var u = -K * error;
    
    // Check stability
    Console.WriteLine($"Max eigenvalue: {eigenvalues.Max().Real:F3}");
    if (eigenvalues.All(e => e.Real < 0))
        Console.WriteLine("System is stable");
}

// EKF for robot localization with encoder + IMU fusion
var ekf = new ExtendedKalmanFilter(new Unicycle2DModel())
{
    // Initial state [x, y, theta]
    InitialState = new[] { 0.0, 0.0, 0.0 },
    
    // Process noise (model uncertainty)
    ProcessNoise = new[] { 0.01, 0.01, 0.001 },
    
    // Measurement noise (sensor uncertainty)
    MeasurementNoise = new[] { 0.1, 0.1, 0.05 }
};

// Main estimation loop
while (true)
{
    // Prediction from motion model
    ekf.Predict(
        input: new { LinearVel = encoderVel, AngularVel = gyroRate },
        dt: 0.01,
        processNoise: noiseQ
    );
    
    // Update with measurements
    ekf.Update(
        measurement: new { EncoderDist = distance, GyroHeading = heading },
        measurementNoise: noiseR
    );
    
    var state = ekf.State;
    Console.WriteLine($"Position: ({state[0]:F2}, {state[1]:F2}), Heading: {state[2]:F1}°");
    Console.WriteLine($"Covariance (position): {Math.Sqrt(ekf.Covariance[0, 0]):F4}");
}

// Complementary filter for IMU fusion
var filter = new ComplementaryFilter
{
    Alpha = 0.98,  // Weight for gyroscope (higher = trust gyro more)
    Beta = 0.02    // Weight for accelerometer
};

// Fast update at IMU rate (200+ Hz)
filter.Update(
    gyroscopeData: new Vector3(gx, gy, gz),
    accelerometerData: new Vector3(ax, ay, az),
    magnetometerData: new Vector3(mx, my, mz),  // optional
    dt: 0.005
);

var attitude = filter.GetAttitude();  // Euler angles or quaternion

// 5th-order polynomial with zero acceleration at endpoints
var trajectory = new QuinticTrajectory(
    startPos: 0, endPos: 100,
    startVel: 0, endVel: 0,
    startAcc: 0, endAcc: 0,
    duration: 5.0
);

for (double t = 0; t <= 5.0; t += 0.01)
{
    double pos = trajectory.GetPosition(t);
    double vel = trajectory.GetVelocity(t);
}