#dsp #Filter-Design #matlab #y2023

Sensor fusion is the process of combining data from different sensors to improve the accuracy and reliability of the result. In this project, you will implement a Kalman filter to fuse data from two sensors: a GPS and an accelerometer.

When we make some measurements, they are not 100% accurate at all. Measurement errors can be systematic error (bias) or random errors. Systematic errors are predictable and can adjust them by adding bias value. But random errors are non-predictable, and they increase the variance of the measurements (increase the uncertainty). In nature, can assume measurement errors are white noises (Follow normal distribution). The influence of the variance can be significantly reduced by averaging or smoothing measurements. But for dynamic system measurements (like vehicle speed detection), averaging not provide a good result. Then the requirement of the prediction come to the picture. If can predict the next state of a system, using previous measurements, the error covariance between prediction and estimation can be minimized. This principle is uses in Kalman filter.
Kalman filter has two main operations.

1. Current State estimate (State update)
2. Next state estimate (State Prediction)
   To above two operations, Kalman filter has five main equations as three for current state update and two for next state predict. The operation can be described from below block diagram:

In Kalman filter, the next state of a system is calculated by using the dynamic model (or State Space Model) of a system. For example, in one dimensional vehicle speed estimation system, dynamic model can be derive using newton motion equation. Most of time dynamic model is a set of differential equations. Other words, the dynamic model describes the relationship between the input and output of the system. In state update block, update the predicted state of the system, by considering the Kalman gain and input measurement value.

Kalman gain is the block that specialized Kalman filter from other filters such as 𝛼, 𝛽,𝛾 filter. Kalman gain is calculate by considering the measurement uncertainty and the prediction uncertainty. Kalman gain is computed as minimize the uncertainty of update state.

Therefore, Kalman filter uses current measurements and the uncertainty of measurement for prediction of next state of the system. Also, Kalman filter gain always try to minimize the uncertainty of the updated state of the system. So Kalman filter is a optimal filter.

Implement the Kalman filter to estimate the true position of the object from the noisy measurements.
Ans: kalman filters has two phases.

1. State Update
2. State Predict
   These are the equations that uses to compute kalman filter.
   

Sensor fusion can be done using several architectures. For this project, used below architecture. Here, accelerometre measurements are used to prediction stage and accelerometer uncertancy used as state prediction variance. GPS measurements are used to update the state.

% Initialization of initial position
 estimate_position = zeros(1,length(t));
 estimate_position(1) = 0;
 % Initialization of variances
 %generaly Accelerometer measurement uncertainty is less than GPS uncertainty
 p = 0.5; %Accelerometer measurement uncertainty
 p = p + (dt.^2).*p; % Estimate uncertainty ---eq.5
 q = 0.5; %process Noise
 r = 5; % GPS uncertainty
 for i = 2:length(t)
    
    %------State Update--------
    K = p/(p+r); % Kalman gain ---eq.3
    p = (1-K).*p; % Covariance Update --eq.2
    estimate_position(i) = estimate_position(i-1)+K.*(GPS_Measurement(i)-estimate_position(i-1)); 
    %------State Prediction------
    v = AccMeter_Measurement(i);
    s = estimate_position(i-1) + dt.*v; % State Extrapolation -- eq.4
    p = p+q; %Covariance Extrapolation ----eq.5
 end
 figure;
 hold on 
plot(estimate_position);
 5
plot(actual_position);
 plot(GPS_Measurement);
 hold off;
Copy

Plot the true position, the noisy measurements, and the estimated position to visualize the performance of Kalman filter.

%Visualization of Position
 figure;
 plot(t,actual_position,'k-','LineWidth', 2, 'DisplayName','Actual Position');
 hold on;
 plot(t,estimate_position,'g--','LineWidth', 2, 'DisplayName','Estimated Position');
 scatter(t,GPS_Measurement,'b', 'filled', 'DisplayName','GPS Measurements');
 xlabel('Time');
 ylabel('Posiotion');
 title('Kalman Filter Estimation of Kalman filter to fuse data from two sensors: a GPS and an accelerometer '
 legend('show');
 grid on;
Copy

%Visualization of Error
 figure;
 hold on;
 plot(t,abs(GPS_Measurement-actual_position),'g-','LineWidth',1,'DisplayName','GPS Measurement Error'
 plot(t,abs(actual_position-estimate_position),'b-','LineWidth',1,'DisplayName','Estimation Error'
 xlabel('Time'); 
ylabel('Error');
 title('Measurements Errors');
 legend('show'); grid on;
 hold off;
Copy

In this mini-project, I designed and implemented a Kalman filter to demonstrate sensor fusion and evaluated the results. The objective was to obtain the position of an object as a required measurement. In practice, GPS data often contain significant noise and may not be available at all times. On the other hand, accelerometers are highly reliable, and their measurements are available most of the time. As accelerometers measure speed, I used a Kalman filter to combine both GPS and accelerometer measurements.

Sensor fusion can be achieved using various methods. However, in this project, accelerometer measurements were used to predict the position, while GPS measurements were used to update the position. The results demonstrated that sensor fusion can reduce the impact of GPS noise on position measurements. However, the implemented Kalman filter was unable to detect velocity changes (or accelerations). To enhance performance, additional system dynamics equations can be included.