This document describes using a modified Kalman filter to estimate vehicle velocity from noisy acceleration and velocity sensor measurements. A standard Kalman filter can cause drift over time. The modified filter automatically converges to eliminate drift without requiring characterization of offset errors. It uses a single state variable rather than separate variables for position, velocity, etc. Simulation results show the modified filter estimates velocity more accurately with no drift compared to a standard Kalman filter. The modified filter also has advantages in easier computation, implementation on lower power processors, and systematic design of gains.
Velocity Estimation from noisy Measurements - Sensor fusion using modified Kalman filter
1. Velocity Estimation from noisy
Measurements
Sensor fusion using modified Kalman filter
www.controltrix.com
copyright 2011 controltrix corp www. controltrix.com
2. Objective
Consider a vehicle moving
• Desired to measure the velocity accurately
• Velocity is directly measured but is noisy
• Acceleration also measured using onboard accelerometers
• Integrating acceleration data gives velocity
• Offset errors in acc./random walk cause drift in velocity
Standard solution
• Kalman filter with optimal gain K for sensor data fusion
• Estimate by combining velocity and acc. measurement
copyright 2011 controltrix corp www. controltrix.com
3. Problem specifics
• Acceleration and velocity are measured using noisy sensor
• Direct velocity measurement is noisy
( v m/s)
• Acceleration is measured with
a = 0.1 m/s2
offset = 0.2 m/s2 (DRIFT)
Superposed sine wave drive
Amplitude A = 3 m/s2,
frequency f = 0.05 Hz
Sample time Ts = 0.1 s
• Simulated time = 200s - 400s
copyright 2011 controltrix corp www. controltrix.com
4. Measured velocity noisy data
(True velocity is smooth sine wave of amp 10, period 20 s)
copyright 2011 controltrix corp www. controltrix.com
5. Advantages
• No matrix calculations
• Easier computation, can be easily scaled
• Equivalent to Kalman filter structure (easily proven)
• No drift (the error converges to 0)
• Estimate accelerometer drift in the system by default
• Drift est. for calib. and real time comp. of accelerometers
copyright 2011 controltrix corp www. controltrix.com
6. Advantages.
• Can be modified easily to make tradeoff between drift
performance (convergence) and noise reduction
• Systematic technique for parameter calculations
• No trial and error
copyright 2011 controltrix corp www. controltrix.com
7. Comparison
Sl No metric Kalman Filter Modified Filter
1. Drift •Drift is a major problem •Guaranteed automatic convergence.
(depends inversely on K) •No prior measurement of offset and
•Needs considerable characterization required.
characterization.(Offset, •Not sensitive to temperature induced
temperature calibration variable drift etc.
etc).
2. Convergence •Non-Zero measurement •Always converges
and process noise •No assumptions on variances required
covariance required else •Never leads to a singular solution
leads to singularity
3. Method •Two distinct phases: •Can be implemented in a few single
Predict and update. difference equation or even in
continuum.
copyright 2011 controltrix corp www. controltrix.com
8. Comparison.
Sl No metric Kalman Filter Modified Filter
4. Computation •Need separate state •Highly optimized computation.
variables for position, •Only single state variable required
velocity, etc which adds more
computation.
5. Gain value •In one dimension, •Gains based on systematic design
/performance •K = process noise / choices.
measurement noise. dt •The gains are good though
• ‘termed as optimal’ suboptimal (based on tradeoff)
6. Processor req. •Needs 32 Bit floating point •Easily implementable in 16 bit
computation for accuracy fixed point processor 40
and plenty of MIPS/ MIPS/computation is sufficient
computation
Note: The right column filter is a super set of a standard Kalman filter
copyright 2011 controltrix corp www. controltrix.com
9. Sim results std Kalman filter
velocity estimation error (v^ - v) vs time
copyright 2011 controltrix corp www. controltrix.com
10. Sim results of proposed solution
error = v^ – v vs time
copyright 2011 controltrix corp www. controltrix.com
11. Thank You
consulting@controltrix.com
copyright 2011 controltrix corp www. controltrix.com
