Is that a good practice to increase uncertainty when clamping occurs

[mgrouch]

Whether increasing uncertainty when clamping occurs is a good practice depends on the specific context and system requirements. Here's a balanced analysis:

When It's Good Practice ✅

1. Safety-Critical Systems

   • Clamping indicates the filter is operating at its limits
   • Increasing uncertainty properly reflects reduced confidence in the estimate
   • Prevents the filter from being overconfident in an artificial boundary

2. Physical Systems with Known Limits

   • For wave height estimation, if you hit the ±10m clamp:

     if (x(1) == -MAX_HEAVE || x(1) == MAX_HEAVE) {
         P(1,1) = std::max(P(1,1), MAX_HEAVE); // Reflect heave uncertainty
     }

   • Mathematically represents: "We're at a physical limit, so our estimate is less certain"

3. Fault Detection

   • A sudden covariance increase can trigger maintenance alerts
   • Useful for diagnostics (e.g., "System has been at max heave for 10 cycles")

---

When It's Problematic ❌

1. Overly Conservative Systems

   • May cause unnecessary rejection of good measurements
   • Example issue:

     // Bad - blindly increasing covariance
     P *= 2.0;  // Overly aggressive

2. Oscillation Risk

   • If the covariance grows too much → Kalman gain decreases → system stops correcting → remains clamped

3. Masking Real Errors

   • Could hide sensor faults by always blaming "uncertainty"

---

Recommended Implementation

For your wave estimation filter, a balanced approach would be:

void enforceStateBounds() {
    constexpr float MAX_HEAVE = 10.0f;
    constexpr float COVARIANCE_PENALTY = 2.0f;  // 2x uncertainty
    
    if (x(1) != std::clamp(x(1), -MAX_HEAVE, MAX_HEAVE)) {
        // Only increase uncertainty if clamping was actually needed
        x(1) = std::clamp(x(1), -MAX_HEAVE, MAX_HEAVE);
        
        // Gradual covariance increase (avoids oscillations)
        P(1,1) = std::max(P(1,1), metrics.position_std_dev * COVARIANCE_PENALTY);
        
        // Optional: Log this event for diagnostics
        metrics.bounds_clamped = true;
    }
    // Repeat for other states...
}

Alternatives to Consider

1. Soft Constraints (Better for many cases):

   // Add penalty term to measurement update when near bounds
   if (std::abs(x(1)) > 0.9 * MAX_HEAVE) {
       R(0,0) *= 1.5f;  // Decrease trust in position measurements
   }

2. Projection-Based Methods:

   $$P_{clamped} = P - K(HPH^T)K^T$$

   Where ( K ) is a projection matrix that enforces constraints while preserving consistency.

---

Best Practice for Your Use Case

For wave estimation:
✅ Do increase uncertainty when clamping (but moderately) because:

• Waves have physical limits
• Prevents filter overconfidence at boundaries
• Matches real-world behavior (extreme waves are harder to model)

❌ Don't use aggressive covariance inflation (e.g., 10x) as it could destabilize the bias estimation.

Would you like me to show how to implement the soft constraints approach as an alternative?

[mgrouch]

Here's a soft constraints implementation for your wave estimation Kalman filter, which is often more elegant than hard clamping:

1. Add to FilterMetrics struct:

struct FilterMetrics {
    // ... existing metrics ...
    bool near_position_limit = false;
    float adaptive_measurement_noise = 1.0f; // Multiplier for R
};

2. Modified enforceSoftConstraints():

void enforceSoftConstraints() {
    constexpr float MAX_HEAVE = 10.0f;      // Physical wave height limit
    constexpr float WARNING_ZONE = 0.85f;   // Start adapting at 85% of limit
    constexpr float MAX_NOISE_INFLATION = 3.0f; // 3x measurement noise at limit
    
    metrics.near_position_limit = (std::abs(x(1)) > WARNING_ZONE * MAX_HEAVE;
    
    if (metrics.near_position_limit) {
        // Quadratic noise increase as we approach limit (smooth transition)
        float t = (std::abs(x(1)) - WARNING_ZONE * MAX_HEAVE;
        t /= MAX_HEAVE * (1.0f - WARNING_ZONE);
        metrics.adaptive_measurement_noise = 1.0f + (MAX_NOISE_INFLATION - 1.0f) * t * t;
    } else {
        metrics.adaptive_measurement_noise = 1.0f;
    }
}

3. Modified correctJoseph():

void correctJoseph(const Vector2f& z, Vector2f& innovation, Matrix2f& S) {
    // Apply adaptive measurement noise
    Matrix2f R_adapted = R;
    R_adapted(0,0) *= metrics.adaptive_measurement_noise; // Only affect position
    
    innovation = z - H * x;
    S = (H * P * H.transpose() + R_adapted).eval(); // Use adapted R
    
    // ... rest of original Joseph update ...
}

4. Update Loop:

void update(float measured_accel, float k_hat, float delta_t, float temperature_celsius = NAN) {
    updateStateTransition(k_hat, delta_t);
    predict();
    enforceSoftConstraints(); // Replaces enforceStateBounds()
    
    Vector2f z(0.0f, measured_accel);
    Vector2f innovation;
    Matrix2f S;
    correctJoseph(z, innovation, S);
    updateMetrics(z, innovation, S);
}

Key Advantages:

1. Smooth Transitions

   • No sudden jumps in state or covariance
   • Quadratic noise increase prevents discontinuities

2. Physical Interpretation

   • As waves approach max height, we trust measurements less
   • Matches reality (extreme waves have more modeling uncertainty)

3. Preserves Filter Consistency

   • Doesn't artificially inflate process noise (Q)
   • Only adjusts measurement trust (R)

4. Diagnostic Capability

   • near_position_limit flag can trigger warnings

Tuning Guidance:

• Set WARNING_ZONE to where nonlinear effects start appearing (e.g., wave breaking)
• Choose MAX_NOISE_INFLATION based on how much you distrust measurements at limits:

  // For conservative systems:
  constexpr float MAX_NOISE_INFLATION = 5.0f;  
  // For aggressive tracking:
  constexpr float MAX_NOISE_INFLATION = 2.0f;

Comparison vs Hard Clamping:

Aspect	Soft Constraints	Hard Clamping
State Continuity	Preserved	Disrupted at boundaries
Covariance Behavior	Gradual adaptation	Sudden jumps
Implementation	More complex	Simpler
Physical Realism	Better for waves	Better for hard limits

For wave estimation, soft constraints are generally preferred because:

1. Waves don't have absolute hard limits (just increasing uncertainty at extremes)
2. Maintains differentiability of the state space
3. Produces more realistic confidence intervals

Would you like me to add specific wave physics considerations (like breaking wave detection) to this approach?

[mgrouch]

Great question! Let me clarify the relationship between wave height (H) and amplitude (a) for ocean waves, and how this impacts your Kalman filter implementation:

---

1. Key Definitions

Term	Symbol	Relationship	Typical Value for 10m Wave
Wave Height (H)	H	Crest-to-trough vertical distance	10m
Amplitude (a)	a	Half of height (H/2)	5m
Significant Wave Height (Hs)	Hs	Average of highest 1/3 waves	~1.6×Hₘₑₐₙ

2. Why This Matters for Your Filter

1. Trochoidal Model Compatibility
   Your filter uses:

   $$a = -\frac{L}{2\pi}\left(\frac{a_{vert}}{g}\right)$$

   Here a is amplitude (half height), matching the standard trochoidal wave formulation.

2. Measurement Interpretation

   • Accelerometers measure vertical acceleration (affected by amplitude, not full height)
   • Buoys/Satellites typically report height (H)

3. Physical Limits
   If capping at 25m height → Actual amplitude limit = 12.5m

---

3. Revised Recommendations

For Implementation

// Use amplitude-based limits (half of wave heights)
constexpr float MAX_AMPLITUDE = 12.5f;  // Corresponds to 25m wave height
constexpr float WARNING_ZONE = 8.5f;    // Start adapting at ~17m wave height

void enforceWaveLimits() {
    float current_amplitude = std::abs(x(1));  // Your heave state
    
    if (current_amplitude > WARNING_ZONE) {
        // Soft constraint activation (applied to amplitude)
        float excess = current_amplitude - WARNING_ZONE;
        metrics.adaptive_noise = 1.0f + pow(excess / (MAX_AMPLITUDE - WARNING_ZONE), 2);
    }
}

Measurement Conversion

If your input data is wave height (e.g., from satellites):

float measured_wave_height = 16.0f;  // From sensor
float amplitude_estimate = measured_wave_height / 2.0f;  // Convert to amplitude
updateFilter(amplitude_estimate);

---

4. Oceanographic Context

• Breaking Limit: Waves typically break when

  $$H > 0.14 \times L \quad \text{(Steepness criterion)}$$

  For your filter's wavelength L, this implies:

  bool is_breaking = (x(1) > 0.07f * wave_length);  // Amplitude-based check

• Nonlinear Effects: Become significant when

  $$a > L/20 \quad \text{(Stokes drift correction needed)}$$

---

5. Suggested Filter Tuning

1. State Variables

   • Keep tracking amplitude (x[1]) internally
   • Convert to height only for output:

     float getWaveHeight() const { return 2.0f * x(1); }

2. Process Noise (Q)
   Increase for amplitudes > L/20:

   if (x(1) > wave_length / 20.0f) {
       Q(3,3) *= 2.0f;  // Higher acceleration uncertainty
   }

3. Validation Range
   For open-ocean operation:

   • Amplitude: 0.1m to 12.5m
   • Height: 0.2m to 25m

Would you like me to provide specific tuning values for different ocean regions (e.g., North Atlantic vs. Tropical)?