Covariance Matrix

A covariance matrix Σ is always symmetric (Σ = Σᵀ). This means the covariance between variable i and variable j is identical to the covariance between variable j and variable i. This property halves the number of unique parameters that need to be stored and estimated.

• Mathematical Guarantee: This follows directly from the definition of covariance: Cov(X, Y) = Cov(Y, X).
• Practical Implication: In a state vector with n dimensions, you only need to estimate n(n+1)/2 unique elements instead of n², reducing computational load.

A valid covariance matrix is positive semi-definite (PSD). This means for any non-zero real vector z, the quadratic form zᵀΣz ≥ 0. This property guarantees that the calculated variances (which lie on the diagonal) are always non-negative and that the matrix represents a plausible, non-inverted uncertainty ellipsoid.

• Consequence of Violation: If a numerical error causes the matrix to lose this property (e.g., becoming indefinite), the Kalman filter can compute negative variances, leading to immediate numerical divergence and filter failure.
• Enforcement: Algorithms like the Joseph form update or square-root filtering are used to maintain this property numerically.

The elements on the main diagonal of the covariance matrix Σᵢᵢ represent the variance of the corresponding state variable. The variance is the square of the standard deviation (σ²), quantifying the absolute uncertainty in that dimension.

• Example: In a robot pose state [x, y, θ]ᵀ, the element Σ[1,1] is the variance of the x-position (σₓ²), and Σ[3,3] is the variance of the heading θ (σ_θ²).
• Physical Meaning: A large diagonal value indicates high uncertainty or poor observability of that specific state component.

The off-diagonal elements Σᵢⱼ (where i ≠ j) represent the covariance between two different state variables. This value indicates the degree and direction of their linear relationship as estimated by the filter.

• Positive Covariance: If variable i increases, variable j is also likely to increase (e.g., x-position and x-velocity).
• Negative Covariance: If variable i increases, variable j is likely to decrease.
• Zero Covariance: The two variables are estimated to be linearly uncorrelated. However, they may still be statistically dependent in a non-linear way.

The covariance matrix defines an uncertainty ellipsoid in the state space. The eigenvectors of the matrix define the ellipsoid's orientation, and the eigenvalues define its extent along those principal axes. This geometric interpretation is central to visualizing estimation confidence.

• Mahalanobis Distance: The distance of a state x from the mean μ, normalized by the covariance Σ, is d² = (x - μ)ᵀΣ⁻¹(x - μ). Points with the same Mahalanobis distance lie on the surface of the same ellipsoid.
• Sensor Planning: In robotics, this ellipsoid is used to evaluate the quality of a state estimate and plan motions or sensor viewpoints to reduce its size.

If a state vector x with covariance P is transformed by a linear function y = Ax, the covariance of y is given by APAᵀ. This is the fundamental linear covariance propagation rule used in the prediction step of the Kalman filter.

• Derivation: Comes directly from the linearity of expectation and the definition of covariance.
• Non-Linear Extension: For non-linear systems (f(x)), this is approximated using the Jacobian F of f evaluated at the mean, leading to the update FPFᵀ. This is the core of the Extended Kalman Filter (EKF) prediction step.

In a Kalman Filter, the covariance matrix P is central to the predict-update cycle.

• Prediction: The state covariance is propagated forward using the process model, increasing uncertainty.
• Update: When a new measurement arrives, the Kalman Gain is computed using the covariance. This gain determines how much to trust the new measurement versus the prior prediction. A large measurement covariance (high sensor noise) results in a small gain, weighting the prediction more heavily.
• The updated covariance is then reduced, reflecting the increased confidence from incorporating the new data.

In VIO, a camera and an IMU are fused. The covariance matrix here is block-structured.

• Diagonal blocks represent uncertainty in position, orientation (quaternion or rotation matrix), velocity, and sensor biases.
• Off-diagonal blocks capture critical correlations, such as between orientation error and velocity estimate, or between gyroscope bias and orientation. Ignoring these correlations leads to suboptimal fusion and increased drift.
• The filter uses this full covariance to correctly weight the high-frequency, drift-prone IMU data against the low-frequency, absolute-but-sometimes-lost visual measurements.

A robot's estimated pose (x, y, θ) has a 3x3 covariance matrix. This can be visualized as an uncertainty ellipsoid (or ellipse for 2D position).

• The eigenvectors of the covariance matrix define the axes of the ellipse.
• The eigenvalues define the lengths of these axes (the variance along each principal direction).
• A long, narrow ellipse indicates the robot is more certain of its position along one axis than another (e.g., certain of location along a hallway wall, but uncertain of distance from it). This is used in motion planning to avoid collisions under uncertainty.

When a robot observes a landmark, it must decide if it's a new landmark or a previously seen one (data association). The Mahalanobis distance is used, which relies on the covariance matrix.

• It measures the distance between an expected measurement (based on the state estimate) and the actual sensor reading, normalized by the combined uncertainty (state covariance + sensor noise covariance).
• Mathematically: d² = (z_expected - z_actual)^T * S^{-1} * (z_expected - z_actual), where S is the innovation covariance.
• If d² exceeds a threshold (e.g., a chi-square distribution quantile), the measurement is rejected as an outlier. This prevents incorrect loop closures from corrupting the map.

In graph-based SLAM (e.g., using g2o or GTSAM), constraints between poses and landmarks have associated covariance matrices acting as information matrices (inverse of covariance).

• A high covariance (low information) constraint is given less weight in the optimization.
• A low covariance (high information) constraint is trusted more. For example, a very precise lidar scan-match constraint will have a small covariance and will pull the solution strongly.
• The estimated covariance of the entire graph after optimization provides a measure of the map's overall accuracy and the robot's final positional uncertainty.

When a robot moves, uncertainty grows. The covariance matrix is propagated using the process model Jacobian F.

• The update is P_new = F * P_old * F^T + Q, where Q is the process noise covariance.
• Q models uncertainty from control inputs (e.g., wheel slip) and unmodeled dynamics.
• The term F * P_old * F^T rotates and stretches the uncertainty ellipse according to the robot's motion. For example, moving forward increases uncertainty in the forward direction more than sideways. This predicted covariance is essential for Model Predictive Control (MPC) to plan trajectories that are robust to state uncertainty.

An optimal recursive algorithm for estimating the state of a linear dynamic system from noisy measurements. It operates in a two-step cycle:

• Prediction: Projects the current state and its covariance forward using a process model.
• Update: Corrects the prediction with a new measurement, optimally blending the prediction and observation based on their respective covariance matrices (prediction and measurement noise). The result is a new state estimate and a reduced covariance matrix representing the updated uncertainty.

The general probabilistic framework underlying state estimation. It recursively applies Bayes' theorem to compute the posterior probability distribution of the system's state given all previous measurements. The Kalman filter is a specific implementation for linear Gaussian systems. The covariance matrix is the representation of the uncertainty (the second moment) of this posterior distribution when it is assumed to be Gaussian.

A statistical distance metric that measures how many standard deviations a point is from the mean of a distribution, accounting for correlations between variables. It is calculated using the inverse of the covariance matrix. In state estimation, it is crucial for:

• Outlier Rejection: Identifying and discarding sensor measurements that are statistically improbable given the current state estimate.
• Data Association: Matching new observations (e.g., LiDAR points) to existing map features or tracks.

A bipartite graphical model used to represent the factorization of a complex probability distribution, such as the one solved in Simultaneous Localization and Mapping (SLAM). Factors represent constraints (from measurements or prior knowledge) and variables represent unknown states. The system's overall covariance is embedded in the structure of this graph. Solving the graph via optimization (e.g., Maximum A Posteriori estimation) yields the most probable state and implicitly recovers the covariance around the solution.

A property of a dynamic system that determines whether its internal state can be uniquely inferred from a sequence of external measurements. If a state variable is unobservable, its uncertainty (variance in the covariance matrix) cannot be reduced by the sensor data, leading to unbounded growth in estimation error. Analyzing the observability of a sensor fusion setup (e.g., VIO) is critical for designing robust estimation systems.

A sophisticated variant of the Kalman filter that estimates the error in a nominal state rather than the full state itself. The primary advantages are:

• Numerical Stability: The error state is typically small, avoiding issues with large rotations in 3D space.
• Linearization Benefits: The error dynamics are often more linear than the full state dynamics, leading to better performance for highly nonlinear systems like attitude estimation. The ESKF maintains a covariance matrix representing the uncertainty in the error state, which is then used to correct the nominal state.