Sensor Fusion : Extended Kalman Filter and Motion Estimation

This app simulates target tracking using different motion models and EKF to estimate positions and sensor biases with visual feedback.

This app provides a flexible framework for simulating target tracking using multiple motion models, sensor fusion, and the Extended Kalman Filter (EKF) for state estimation. The app can handle various motion dynamics, including Constant Velocity (CV), Constant Acceleration (CA), and the Singer acceleration model, and it offers flexibility in adjusting process noise and sensor settings. It is designed for researchers and engineers interested in understanding target motion and sensor calibration, offering visualizations of the target’s trajectory, estimated states, and sensor biases.

Simulation settings:

• Time Settings: The simulation is configured to run for a specified duration with a chosen time step.
• State Vector Setup: The state vector of the vehicle motion contains:

Motion Equations:

The motion equations define how the state evolves over time based on the selected model:

• Constant Velocity (CV) Model:

• Constant Acceleration (CA) Model:

• Singer Model (with correlation time constant Tau): α = 1/τ

Adding Process Noise:

To simulate uncertainties in the target's motion, the process noise covariance matrix Q is used. The general form of Q is:

Adding Sensors:

Sensors provide noisy measurements of the target's position using range and bearing. The measurement model relates the target's true position to the sensor’s observations:

where xs,ys are the sensor's position, and θs is its orientation.

EKF Settings:

The app employs the Extended Kalman Filter (EKF) to estimate the target’s state and sensor biases. The EKF consists of two key steps:

• Prediction:

• Update: The Kalman Gain is calculated using the measurement Jacobian Hk

• The state and covariance are updated using the sensor measurements:

Final Output:

The app visualizes both the true trajectory of the target and the estimated trajectory produced by the EKF. Additionally, it tracks and plots the convergence of sensor positions and orientations over time, allowing users to observe how the EKF corrects for initial uncertainties and biases.

Advantages:

• Multiple Motion Models: The app supports different motion models (CV, CA, and Singer model), allowing users to select the most appropriate dynamics for their scenario.
• Extended Kalman Filter (EKF): The app employs EKF for state estimation, which is well-suited for systems with non-linear measurements.
• Sensor Fusion: The app integrates data from multiple sensors (range and bearing), enabling the estimation of both target states and sensor biases.
• Process and Measurement Noise Handling: Users can add Gaussian noise to both process dynamics and sensor measurements to simulate real-world uncertainties.
• Interactive Visualization: The app provides dynamic plots to compare true trajectories, estimated trajectories, and sensor bias convergence over time.
• Editable Sensor Properties: You can modify sensor properties (name, position, orientation, noise) at any time during the simulation.
• Save and Load Scenarios: The app allows you to save specific scenarios for later use and also provides the option to import previously saved scenarios for simulation and analysis.

author : Alireza Esmailnezhad

Email : [e-mail]

created : 9/5/2024 - version 1.2.0