/** * Configuration options for the Multi-Sensor EKF. */ export interface MultiSensorEKFOptions { /** * Half-life for uncertainty growth in milliseconds. * The state uncertainty doubles every halfLifeMs when no measurements arrive. * Lower values = faster decay of old estimates. * @default 10000 (10 seconds) */ halfLifeMs: number; } /** * Default options for MultiSensorEKF. */ export declare const DEFAULT_EKF_OPTIONS: MultiSensorEKFOptions; /** * Multi-sensor Extended Kalman Filter for position fusion. * * Combines position measurements from multiple sources (GPS, VPS, anchors) and * displacement measurements from PDR using a constant-velocity motion model. * * Key features: * - All methods take explicit timestamps (no Date.now() inside) * - Confidence-weighted fusion via measurement noise parameter * - Half-life decay of old estimates via process noise scaling * - Supports both absolute position and relative displacement updates * * State vector: [latitude, longitude, velocity_lat, velocity_lon] * * @example * ```ts * const ekf = new MultiSensorEKF({ halfLifeMs: 10000 }); * * // Initialize with first position * ekf.updatePosition(43.123, -79.456, 0.0001, 0); * * // Predict forward * ekf.predict(1000); * * // Update with new GPS measurement (low confidence = high noise) * ekf.updatePosition(43.124, -79.455, 0.001, 1000); * * // Get fused position * const pos = ekf.getPosition(); // { latitude: ~43.1235, longitude: ~-79.4555 } * ``` */ export declare class MultiSensorEKF { #private; constructor(options?: Partial); /** * Predict step: project state forward to the given timestamp. * Increases uncertainty based on elapsed time and half-life configuration. * * @param timestampMs - Current timestamp in milliseconds */ predict(timestampMs: number): void; /** * Update state with a position measurement (GPS, VPS, anchor). * * @param latitude - Measured latitude in degrees * @param longitude - Measured longitude in degrees * @param measurementNoise - Measurement noise variance (higher = less trust) * @param timestampMs - Measurement timestamp in milliseconds */ updatePosition(latitude: number, longitude: number, measurementNoise: number, timestampMs: number): void; /** * Update state with an odometry measurement (PDR displacement). * * Unlike position updates, odometry provides relative displacement rather than * absolute position. The displacement is added directly to the state. * * @param dx - Displacement east in meters (positive = east) * @param dy - Displacement north in meters (positive = north) * @param measurementNoise - Measurement noise variance (higher = less trust) * @param timestampMs - Measurement timestamp in milliseconds */ updateOdometry(dx: number, dy: number, measurementNoise: number, timestampMs: number): void; /** * Get the current position estimate. * @returns Position or null if filter not initialized */ getPosition(): { latitude: number; longitude: number; } | null; /** * Get the current velocity estimate. * @returns Velocity in degrees per second, or null if filter not initialized */ getVelocity(): { vLat: number; vLon: number; } | null; /** * Get the current position uncertainty (trace of position covariance). * Higher values indicate less certainty in the position estimate. * @returns Uncertainty value, or Infinity if filter not initialized */ getUncertainty(): number; /** * Get the last update timestamp. * @returns Timestamp in ms, or null if filter not initialized */ getLastTimestamp(): number | null; /** * Check if the filter has been initialized with at least one measurement. */ isInitialized(): boolean; /** * Reset the filter state. * Call on floor changes, large position jumps, or time gaps. */ reset(): void; }