Kalman Filter Tutorial: MATLAB Implementation with Bayesian NINJAS

Advanced sensor fusion techniques demonstrate how combining multiple sensors produces enhanced capabilities beyond individual sensor limitations. Data-in/data-out fusion applies filters to raw data for reliability improvement, while data-in/feature-out extracts meaningful characteristics. Feature-in/feature-out combines features from multiple sensors to create new features—such as combining ultrasonic depth measurements with camera shape analysis to calculate volumetric size. Feature-in/decision-out represents the most common approach in machine learning, using extracted features for classification tasks. Decision-in/decision-out combines individual sensor decisions to identify behavioral patterns over time. A practical demonstration uses accelerometer, gyroscope, and magnetometer sensors for 3D orientation determination (pitch, yaw, roll). Raw data shows measurements at different scales requiring calibration. The Kalman filter combines accelerometer and gyroscope data to produce stable roll and pitch angles by reducing noise and drift. A tilt compensation unit integrates magnetometer data with these angles to calculate yaw rotation. This example illustrates how individual sensor limitations (drift, noise, magnetic interference) are overcome through strategic fusion, producing reliable 3D positioning essential for IoT applications.

Advanced fusion approaches use learnable networks with attention mechanisms to dynamically combine sensor features. Multiple fusion operators exist: concatenation stacks representations; element-wise operations combine values directly; order statistics extract means, medians, or maxima; learnable fusion uses dedicated sub-networks to discover optimal combination weights. A general framework creates weight matrices representing all possible sensor-sensor interactions, though full matrices require low-rank approximations. Strategic selection depends on priorities: early fusion maximizes recall (detects everything); late fusion maximizes accuracy (eliminates false positives); mid-level fusion offers optimal performance but is challenging to train. Sensor complementarity is key—cameras excel at classification while lidar excels at 3D localization. Robust systems must handle sensor failures by training networks to recognize unreliable information during adverse conditions, potentially using separate reliability assessment networks. Ablation studies assess individual sensor contributions to specific tasks.

Orientation estimation determines device attitude (pitch, roll, heading) relative to reference frames. Dead reckoning integrates gyroscope angular velocity measurements over time but accumulates error due to drift and requires precise initial alignment. Sensor fusion algorithms blend sensor outputs by determining trust percentages: complementary filter uses manual weight assignment between accelerometer and gyroscope; Kalman filter continuously calculates adaptive trust levels (Kalman gain) based on real-time conditions. More sophisticated filters like Mahony and Madgwick extend these principles with advanced mathematical modeling. All approaches aim to combine sensor strengths while minimizing individual weaknesses, creating reliable orientation estimates for applications like motion capture suits and aerospace navigation systems.

Sensor fusion enhances system reliability through redundancy—identical sensors provide backup when one fails, while algorithms can discard inconsistent readings from any single sensor. However, single failure modes affecting all sensors simultaneously require fusing different sensor types (like GPS with atmospheric models) for continued operation. When measurements drop out temporarily (obstruction rather than failure), predictive models based on object behavior can maintain estimates. For unmeasured states, multi-sensor fusion enables estimation impossible for individual sensors—two cameras at different angles can extract 3D distance information that neither could provide alone. These advanced applications demonstrate how sensor fusion transforms raw measurements into robust, comprehensive system understanding.

Sensor fusion involves combining data from multiple vehicle sensors including cameras, radars, lidars, and ultrasonic sensors. The raw output from each sensor is processed using machine learning, then fused together and interpreted using AI to understand the driving scene. This enables vehicles to achieve a 360° peripheral view of their environment, detect obstacles and potential dangers 24/7 in any condition and drivable area.

Particle filtering, or Monte Carlo Localization (MCL), is a probabilistic algorithm that represents a robot's uncertain position as a set of weighted particles (samples) and recursively updates these particles through motion prediction and measurement correction using importance sampling, allowing robots to estimate their location in complex environments without requiring complex analytical probability calculations.

A particle filter is a recursive Bayesian estimation technique for dynamic systems that uses a set of weighted particles to approximate probability distributions. Unlike Kalman filters, it handles nonlinear functions and arbitrary distributions without Gaussian assumptions. The algorithm operates through three core steps: (1) Prediction - particles are propagated forward using motion models with added noise; (2) Correction - particles receive importance weights based on how well they explain new observations; (3) Resampling - high-weight particles are replicated while low-weight ones are eliminated, implementing survival of the fittest. Each particle represents a state hypothesis (position, orientation) with an associated weight. This nonparametric approach makes particle filters ideal for mobile robot localization in GPS-denied indoor environments, where they serve as the gold standard Monte Carlo localization method.

Particle filters (PF), also called sequential Monte Carlo methods (SMC), are powerful tools for estimating dynamic system parameters or states from measurements. Unlike Kalman filters that assume Gaussian noise, particle filters use probabilistic techniques to assign coefficients to particles and eliminate measurement noise. The algorithm builds upon five foundational topics: Bayesian estimation, sequential importance sampling, noise elimination from measurements, importance sampling, and resampling techniques. These components together form the basis for implementing particle filters in complex dynamic systems.

Particle filtering is a numerical approximation method for solving nonlinear filtering problems where analytical solutions are intractable. The method constructs an interacting particle system where independent copies of the signal process evolve according to stochastic differential equations, with each particle weighted by an exponential martingale representing the likelihood of observations. The normalized filter can be approximated by the weighted empirical measure of a finite particle system, and the Euler-Maruyama method discretizes the particle dynamics. The error between the particle filter approximation and the true filter decays at rate O(√(log n)/n), though the basic particle filter suffers from exponentially growing weight variances over time, motivating advanced variants like branching particle methods for long-time horizon applications.

The particle filter algorithm consists of three sequential steps applied at each time step: (1) Prediction/Sampling: Draw new particle positions from the transition kernel given the previous state, effectively predicting where the system might evolve; (2) Weighting: Compute weights for each particle proportional to the likelihood of the current observation given that particle's state; (3) Resampling: Generate a new set of particles by sampling with replacement from the weighted set, giving higher-weight particles more copies. This creates an evolutionary algorithm where particles with poor fitness (low likelihood) are eliminated while good particles reproduce.

For linear Gaussian systems with additive white noise, the optimal output feedback controller can be obtained by separately solving two problems: the Kalman filter for state estimation and Linear Quadratic Regulator (LQR) for control. This separation principle states that the optimal controller can be designed independently of the estimator, and vice versa. The complete solution involves running the Kalman filter to obtain the estimated state mean, then applying the LQR controller to this estimate. This elegant result works because Gaussian distributions remain Gaussian when passed through linear systems, and the optimal policy depends only on the mean of the state distribution.

Linear Quadratic Gaussian (LQG) control unifies optimal state estimation (Kalman filter) with optimal control (LQR) for partially observable systems. The separation principle states that for LQG problems, the optimal controller can be designed by separately designing an optimal estimator and an optimal controller, then combining them. This powerful property allows independent design of estimation and control components, dramatically simplifying the overall control system design. However, this optimality guarantee holds only for linear systems with Gaussian noise and quadratic costs—once any aspect becomes nonlinear or non-Gaussian, the separation principle no longer ensures optimal performance.

The Linear Quadratic Gaussian (LQG) controller combines an optimal full-state feedback controller (LQR) with an optimal state estimator (Kalman Filter/LQE) to handle systems with Gaussian disturbances and measurement noise; remarkably, due to the separation principle, the combined system retains the exact eigenvalue placements chosen for both the controller and estimator when designed in isolation, allowing engineers to design each component separately while achieving optimal overall system performance.

Linear Quadratic Gaussian (LQG) control is an optimal control strategy that combines Linear Quadratic Regulator (LQR) for optimal feedback control with Kalman filter for state estimation in noisy environments, enabling stabilization and performance optimization when system states cannot be directly measured due to sensor noise or process disturbances.

The Linear Quadratic Gaussian (LQG) controller integrates an optimal LQR controller with a Kalman filter estimator for robust control in noisy environments. The architecture computes control inputs as u = -K*x_hat, where K is the LQR gain and x_hat is the Kalman filter estimate. Two-loop control systems separate fast and slow dynamics: the inner loop handles pitch rate control with proportional control and saturation limits, while the outer loop manages flight path angle setpoints. This hierarchical structure enables independent tuning of different time scales while maintaining overall system stability and meeting performance specifications for complex aerospace applications.