Imran Meah

Student ID- 664509927

Implementation of Multi-sensor fusion using Kalman Filter

ECE 418




















Sensors are used to measure the state (position, temperature, speed) a system. In an ideal
world, sensor would measure exact state of system without any noise. However, In real world, sensor
suffers from both internal and environmental noise. And different sensor suffers from different form of
noise. Sensor noise is not negligible and it could have devastating effect since control system depends
and operates from acquired sensor data. Without reliable state information, the system cant function
properly.
To address this problem, Sensor fusion is utilized to attain more precise state of the system.
Sensor fusion is combining (weighted averaging) of different sensor data to suppress noise. Kalman filter
is ideal for this purpose. `
Kalman filter is predictor-corrector estimator developed in 1960s by Rudolf E. Klmn, it was
used extensively by NASA for Apollo spacecraft and subsequent missions. Using dynamic of a system
and control input, state of the system is predicted. The predicted state is compared to physical
measurement from each sensor. The state of the system is estimated by comparing predicted state to
measurements. Along with state auto covariance is also calculated with is used to compute Kalman
gains.


For this paper, we want to accurately determine a position of a rover. We have position and velocity as
state equation. There are two sensors onboard, one measures velocity which is integrated to find
position. Other measure position directly.
The state equation for rover is defined as followed,


X
n+1
=A X
n
+ B u +W
Where A is state transition matrix
A=(


)
B is state control matrix
B=(

)
And W is noise
W=

(
(

)

)

Two sensor are modeled as
Y
i
= C
i
*X+ sensor_noise

C
1
=[1,0]
C
2
=[1.01,1*dt]

The vector C relates state of the system to measurement. For our purpose , we simulate sensor output
by adding some noise to actual state of the system. Our aim is to remove this random noise using
kalman filter.
Position of rover based on system dynamic only(no sensor input used)

The red line is actual position , whereas the blue line is based on system dynamic only under false assumption there is no
process noise. As we can see, as time progress deviations gets significantly larger.

Lets now look at output of sensor data

Red line is actual states while yellow and black lines represent two sensor data. As we can see the sensor data is randomly
distributed around the actual state of the system.

Kalman Filter with 2 sensors

The black and yellow are sensor output and red is real states while green is kalman filter output.

Analysis of 1 sensor vs 2 sensor Kalman filter implementation



As we can see, the precision improve drastically as more sensor input are added.


Matlab code
% written by Meah



clear all
%% define our meta-variables (i.e. how long and often we will sample)
duration = 100 %how long the simulation runsw
dt = .1; %Sensor sample rate,


%% Define update equations (Coefficent matrices): A physics based model for
where we expect the rover to be[state transition (state + velocity)] + [input
control (acceleration)]
A = [1 dt; 0 1] ; % state transition matrix: expected path of the
rover(state prediction)
B = [dt^2/2; dt]; %input control matrix: expected effect of the input
accceleration on the state.
C = [1 0];
C2 = [1.01,dt];% measurement matrix: the expected measurement given the
predicted state (likelihood)
%C2 = [1,0];
%since we are only measuring position, we set the velocity variable to
%zero.

%% define main variables
u = 1.5; % define acceleration magnitude
Q= [0; 0]; %initized state--it has two components: [position; velocity] of
the Rover
Q_estimate = Q;%x_estimate of initial location estimation of where the Rover
is (what we are updating)
Q_estimate1 = Q;
Q_predictor=Q;
RoverAccel_noise_mag = 4; %process noise: the variability in how fast the
Rover is speeding up (stdv of acceleration: meters/sec^2)
sensor1_noise_mag = 8; %measurement noise: (stdv of location, in meters)
Ez = sensor1_noise_mag^2;% Ez convert the measurement noise (stdv) into
covariance matrix
Ez2 = sensor1_noise_mag^2;% Ez convert the
Ex = RoverAccel_noise_mag^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % Ex convert the
process noise (stdv) into covariance matrix
P = Ex;% estimate of initial rover position variance (covariance matrix)
P2 = Ex;

%% initize result variables
% Initialize for speed
Q_loc = []; % ACTUAL rover path
vel = []; % ACTUAL rover velocity
Q_loc_meas = []; % Rover path as measured by sensor i
Q_loc_meas2 = [];
Q_loc_meas3 = [];

%% simulate what the sensor data sees over time

for t = 0 : dt: duration

% Generate the actual rover path
RoverAccel_noise = RoverAccel_noise_mag * [(dt^2/2)*randn; dt*randn];
Q= A * Q+ B * u + RoverAccel_noise;
% Generate what the sensor see
sensor_noise = sensor1_noise_mag * randn;
sensor_noise2 = sensor1_noise_mag * randn;
y = C * Q+ sensor_noise;
y2 = C2 * Q+ sensor_noise2;
%y3 = C * Q+ sensor_noise2;
Q_loc = [Q_loc; Q(1)];
Q_loc_meas = [Q_loc_meas; y];
Q_loc_meas2 = [Q_loc_meas2; y2];
vel = [vel; Q(2)];
% Q_loc_meas3 = [Q_loc_meas3; y3];
%iteratively plot what the sensor sees
plot(0:dt:t, Q_loc, '-r.') % dyanamic of the rover
plot(0:dt:t, Q_loc_meas, '-k.') %sensor 1 is black
plot(0:dt:t, Q_loc_meas2, '-y.') %sensor 2 is yellow
% plot(0:dt:t, Q_loc_meas3, '-b.') %sensor 2 is blue
axis([0 10 -30 80])
hold on

end



%% Do kalman filtering
%initize estimation variables
Q_loc_estimate = []; % Rover position estimate
Q_loc_estimate1 = [];
vel_estimate = []; % Rover velocity estimate
Q= [0; 0]; % re-initized state
P_estimate = P;
P_mag_estimate = [];
P_mag_estimate2 = [];
predic_state = [];
predic_state1 = [];
predict_state = [];
predic_var = [];
for t = 1:length(Q_loc)
% Predict next state of the rover with the last state and predicted
motion.
Q_predictor = A * Q_predictor + B * u;
predict_state = [predict_state; Q_predictor(1)] ;
end
for t = 1:length(Q_loc)
% Predict next state of the rover with the last state and predicted
motion.
Q_estimate1 = A * Q_estimate1 + B * u;
Q_estimate = A * Q_estimate + B * u;
predic_state = [predic_state; Q_estimate(1)] ;
predic_state1 = [predic_state1; Q_estimate1(1)] ;
%predict next covariance
P = A * P * A' + Ex;
P2 = A * P2 * A' + Ex;
predic_var = [predic_var; P] ;
% predicted sensor measurement covariance
% Kalman Gain For Two Sensor
K = P*C'*inv(C*P*C'+Ez);
K2 = P2*C2'*inv(C2*P2*C2'+Ez2);
% Update the state estimate.
Q_estimate1 = Q_estimate1 + K2 * (Q_loc_meas2(t) - C * Q_estimate1);
Q_estimate = Q_estimate + K * (Q_loc_meas(t) - C * Q_estimate)+K2 *
(Q_loc_meas2(t) - C2 * Q_estimate);
% update covariance estimation.
P = (eye(2)-K*C)*P;
P2 = (eye(2)-K2*C2)*P2;
%Store for plotting
Q_loc_estimate = [Q_loc_estimate; Q_estimate(1)];
Q_loc_estimate1 = [Q_loc_estimate1; Q_estimate1(1)];
vel_estimate = [vel_estimate; Q_estimate(2)];
P_mag_estimate = [P_mag_estimate; P(1)];
P_mag_estimate2 = [P_mag_estimate2; P2(1)];


end

% Plot the results
figure();
tt = 0 : dt : duration;
%plot(tt,Q_loc,'-r.',tt,predict_state,'-b');
plot(tt,Q_loc,'-r.',tt,Q_loc_estimate,'-g.',tt,Q_loc_estimate1,'-y');
%plot(tt,Q_loc,'-r.',tt,Q_loc_meas,'-k.',tt,Q_loc_meas2,'-
y.',tt,Q_loc_estimate,'-g.');
%plot(tt,Q_loc,'-r.',tt,Q_loc_meas,'-k.',tt,Q_loc_meas2,'-y.');
hold on
axis([0 10 -30 80])