Upcoming SlideShare
×

# Report kalman filtering

756 views

Published on

2 Likes
Statistics
Notes
• Full Name
Comment goes here.

Are you sure you want to Yes No
• Be the first to comment

Views
Total views
756
On SlideShare
0
From Embeds
0
Number of Embeds
2
Actions
Shares
0
48
0
Likes
2
Embeds 0
No embeds

No notes for slide

### Report kalman filtering

1. 1. Report on Kalman Filtering Submitted to: Dr. Muhammad Zia Course: Advance Digital Signal Processing (El-725) Submitted by: Muhammad Irfan Anjum
2. 2. Kalman Filtering Introduction Kalman Filter uses a series of measurements over time, and produces estimates of unknown variables that tend to be more precise than those based on a single measurement alone. It operates recursively on streams of noisy input data, to produce a statistically optimal estimate of the underlying system state. It’s a two step process, in first phase estimates of current state variables with their uncertainties are calculated, while in second part these estimates are updated using weighted average after observing output. It operates on real time data, no additional past information is required. It’s different from wiener filter in processing in sense that it can operate on non-stationary data while wiener filter assumes data to be stationary. It allows the parameters to be estimated to evolve with time according to a dynamical model. To study Kalman filter in detail, we must have knowledge of the dynamical signal modeling. An introduction to dynamical signal models is given in following section. Dynamical Signal Models We begin our discussion of signal modeling by using an example of a DC voltage source in presence of AWGN. (1) Fig. 1 True voltage and MVU estimator.
3. 3. where, A is the parameter to be estimated and is the noise introduced by the inaccurate voltmeter. In real systems parameter is not a constant, in fact it varies with time with small variance due to the component aging, temperature and other effects. So we can change the signal model to be as (2) here, is also a function of time, i.e., it varies with time. Successive samples of will not be too different, leading us to conclude that they yield a high degree of correlation. The problem is to estimate correlated process using noisy observations x . Fig. 1 shows the corresponding graphs. Gauss Markov Process The correlated process can be modeled using 1st order Gauss Markov Process. Gauss Markov Process is a simple model to describe correlation between samples. Fig. 2 shows the modeling of to generate Gauss Markov Process s . Equations [3-5] show the generation process of scalar state Gauss Markov Process, its recursive form and mean of the process. (3) (4) (5) where, is the deriving noise having variance  Process can be summarized as 2  . In vector form the vector state Gauss Markov (6) (7) Fig. 2 Dynamical Signal Model used to generate Gauss Markov Process Kalman filter can be portioned into 4 main categories. These include following; i) ii) iii) iv) Scalar state scalar Kalman filter. Scalar state vector Kalman filter. Vector state vector Kalman filter. Extended Kalman filter.
4. 4. Scalar state scalar Kalman filter Gauss Markov Model described in previous section has the form We now describe a sequential MMSE estimator which will allow us to estimate based on the data as increases. Such an operation is known as Kalman filtering. The approach computes the estimator based on the previous time sample and so is recursive is in nature. This is so called Kalman filter. Fig. 3 shows the model of the scalar state scalar Kalman filter. Fig. 3 Kalman filter Mathematically we can describe the filtering process as; Prediction: Minimum Prediction MSE: Kalman Gain: Correction: Minimum MSE
5. 5. Simulation setup To generate the 1st order Gauss Markov Process, the noise variance of the deriving noise is assumed to be 0.1 and the noise introduced by the inaccurate voltmeter was assumed to be of variance 1. We can model our problem as calculating the gain of a random channel i.e. predicting one state only depending upon the previous samples. The correlation coefficient in generating Gauss Markov Process is assumed to be 0.1. Number of samples transmitted was taken to be 100. Fig. 4 shows the results against number of samples. Figs. [4-5] shows the results obtained for scalar Kalman filter. Fig. 4 Scalar state Vector Kalman Filter For the Scalar state Vector Kalman Filter, we have multiple states observation vector . In this case the model is shown to be as and single realization of
6. 6. Fig. 5 where are known and matrices, is vector WGN. The data model is observation or measurement equation. The Kalman filter derived for this setup is exactly the same as for the scalar case; we summarize the results. Prediction Minimum Prediction MSE Matrix (p*p): Kalman Gain Vector (P*1): Correction:
7. 7. Minimum MSE Matrix (p*p): Fig. 6 Vector state Vector Kalman Filter For the vector state Vector Kalman Filter, we have multiple states of observation vector . In this case the model is shown to be as where are known and matrices, and multiple realizations is vector WGN. where is known observation matrix. The data model is observation or measurement equation. The Kalman filter derived for this setup is exactly the same as for the scalar case; we summarize the results. Prediction Minimum Prediction MSE Matrix (p*p): Kalman Gain Vector (P*1): Correction:
8. 8. Minimum MSE Matrix (p*p): Fig. 7 shows the simulation result for the vector state vector Kalman filter. In this example particle tracking problem is considered, having two states the x-position & y-position. Fig. 7 Vector Kalman Filter Extended Kalman Filter Vector state vector Kalman filter can be modified to describe the model of extended Kalman filter, in which state equation or observation equation may be non-linear. A simple example is considered here, that is of vehicle tracking, in which the observations are range estimates and bearing estimates. For the present case, the Gauss Markov Process and the observation or measurement is given as non-liner function of the state variables. s[ n ]  a ( s[ n  1])  Bu [ n ] x[ n ]  h ( s[ n ])  w[ n ]
9. 9. ˆ a ( s[ n  1])  a ( s [ n  1 | n  1])  A[ n  1]  a  s[ n  1] h  s[ n ]  s[ n  1] | s [ n 1 ]  sˆ [ n 1| n 1 ] | s [ n 1 ]  sˆ [ n 1| n 1 ] ˆ h ( s[ n ])  h ( s [ n | n  1])  H [n]  a h  s[ n ] | s [ n ]  sˆ [ n | n 1 ] | s [ n ]  sˆ [ n 1| n 1 ] Prediction: ˆ ˆ s[ n | n  1]  a ( s[ n  1 | n  1]) Minimum Prediction MMSE M [ n | n  1]  A[ n  1] M [ n  1 | n  1] A [ n  1]  BQB T Kalman Gain M [ n | n  1] H [ n ] T K [n]  C [ n ]  H [ n ] M [ n | n  1] H [ n ] T Correction ˆ ˆ ˆ s[ n | n ]  s[ n | n  1]  K [ n ]( x[ n ]  h ( s[ n | n  1])) Minimum MSE M [ n | n ]  ( I  K [ n ] H [ n ]) M [ n | n  1] Fig. 8 shows the particle track using extended Kalman filter. T
10. 10. Fig. 8 Extended Kalman Filter Appendix: Matlab Code; Scalar Kalman Filter %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % PARTICLE TRACKING % USING SCALER KALMAN FILTER %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% N = 100; %number of samples sig_u_sqr = 0.1; % driving noise (control) sig_x_sqr = 0.1; % noise variance of range observation %initialization s = zeros(1, N); x = zeros(1, N); pre_est_s = zeros(1,N); est_s = zeros(1,N); M = zeros(1,N); pre_M = zeros(1,N); K = zeros(1,N); a=1;
11. 11. %initial condiitions est_s(1) = 5; s(1) = 10; M(1) = 100; for n=2:N %particle is moving with constant velocity "1", %with driving noise of sig_u_sqr variance s(n) = a*s(n-1) + sig_u_sqr*randn(1,1); %observing the position with noise variance sig_x_sqr x(n) = s(n) + sig_x_sqr*randn(1,1); %Prediction pre_est_s(n) = est_s(n-1); %Minimum Prediction MSE pre_M(n) = a^2*M(n-1) + sig_u_sqr; %Kalman Gain K(n) = pre_M(n)/(sig_x_sqr + pre_M(n)); %Correction est_s(n) = pre_est_s(n) + K(n)*(x(n) - pre_est_s(n)); %Minimum MSE M(n) = (1 - K(n))*pre_M(n); end %% figure plot(s,'-r') hold on plot(x(2:end), '-b') plot(est_s, '-k') hold off legend('Actual track', 'Observed track', 'Estimated track') xlabel('Samples') ylabel('X position') title('Particle track') grid on figure; plot(M); title('MMSE for scalar Kalman filter'); xlabel('No. of samples'); ylabel('MMSE'); grid on Vector Kalman Filter: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % PARTICLE TRACKING % USING VECTOR KALMAN FILTER %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% del = 1; %time step N = 100; %number of samples
12. 12. sig_u_sqr = 0.001; %driving noise (control) sig_x_sqr = 0.1; %noise variance of range observation sig_y_sqr = 0.1; %noise variance of bearing observation A = [ 1, 0, 0, 0, 0, 1, 0, 0, del, 0 , 1 , 0 , B = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0 ; del; 0 ; 1 ]; 0; 0; 0; 1]; %linear transformation of observation H = [ 1, 0, 0, 0; 0, 1, 0, 0]; %covariance Q = [ 0, 0, 0, 0, 0, 0, 0, 0, matrix of system 0 , 0 ; 0 , 0 ; sig_u_sqr, 0 ; 0 , sig_u_sqr]; %convarince matrix of observation C = [ sig_x_sqr, 0 ; 0 , sig_y_sqr]; %initialization s = zeros(4, N); x = zeros(2, N); pre_est_s = zeros(4,N); est_s = zeros(4,N); M = zeros(4,4,N); pre_M = zeros(4,4,N); K = zeros(4,2,N); %initial values est_s(:,1) = [5;5;0;0]; s(:,1) = [10;-5;-0.2;0.2]; M(:,:,1) = eye(4)*100; I = eye(4); for n=2:N %driving noise vector with same x and y noise variances u = [0;0;sig_u_sqr*randn(1,1);sig_u_sqr*randn(1,1)]; %particle is moving in x-y plane s(:,n) = A*s(:,n-1) + B*u; %observing noise in x and y position w = [sig_x_sqr*randn(1,1);sig_y_sqr*randn(1,1)]; %obseration of x and y coordinates with noise x(:,n) = H*s(:,n) + w;
13. 13. %Prediction pre_est_s(:, n) = A*est_s(:, n-1); %Minimum Prediction MSE Matrix pre_M(:,:,n) = A*M(:,:,n-1)*A' + B*Q*B'; %Kalman Gain K(:,:,n) = pre_M(:,:,n)*H'*(C + H*pre_M(:,:,n)*H')^-1; %Correction est_s(:,n) = pre_est_s(:,n) + K(:,:,n)*(x(:,n) - H*pre_est_s(:,n)); %Minimum MSE Matrix M(:,:,n) = (I - K(:,:,n)*H)*pre_M(:,:,n); end figure plot(s(1,:), s(2,:)) hold on plot(x(1,:), x(2,:), ':g') plot(est_s(1,:), est_s(2,:), '--r') hold off legend('Actual track', 'Observed track', 'Estimated track') xlabel('X position') ylabel('Y position') title('Particle track') grid on Extended Kalman Filter: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % PARTICLE TRACKING % USING EXTENDED KALMAN FILTER %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc clear close all %% del = 1; %time step N = 100; %number of samples sig_u_sqr = 0.001; %driving noise (control) sig_r_sqr = 0.1; %noise variance of range observation sig_b_sqr = 0.1; %noise variance of bearing observation A = [ 1, 0, 0, 0, 0, 1, 0, 0, %convarince Q = [ 0, 0, 0, 0, 0, 0, 0, 0, del, 0 , 1 , 0 , 0 ; del; 0 ; 1 ]; matrix of driving noise 0 , 0 ; 0 , 0 ; sig_u_sqr, 0 ; 0 , sig_u_sqr];
14. 14. %covarince matrix of observation noise Cw = [ sig_r_sqr, 0 ; 0 , sig_b_sqr]; %initializations s = zeros(4, N); x = zeros(2, N); pre_est_s = zeros(4,N); est_s = zeros(4,N); M = zeros(4,4,N); pre_M = zeros(4,4,N); K = zeros(4,2,N); %initial values est_s(:,1) = [5;5;0;0]; s(:,1) = [10;-5;-0.2;0.2]; M(:,:,1) = eye(4)*100; I = eye(4); %% for n=2:N %particle moving in x y plane s(:,n) = A*s(:,n-1) + [0;0;sig_u_sqr*randn(1,1);sig_u_sqr*randn(1,1)]; %observing range and bearing of the particle with noise x(:,n) = [sqrt(s(1,n)^2+s(2,n)^2);atan2(s(2,n),s(1,n))] + ... [sig_r_sqr*randn(1,1);sig_b_sqr*randn(1,1)]; %Prediction pre_est_s(:, n) = A*est_s(:, n-1); %Minimum Prediction MSE Matrix pre_M(:,:,n) = A*M(:,:,n-1)*A' + Q; %extracting rx ry and range from Prediction rx = pre_est_s(1,n); ry = pre_est_s(2,n); R = sqrt(rx^2 + ry^2); %creating H from previously extracted rx ry and range (R) H = [rx/R , ry/R , 0, 0; -ry/R^2, rx/R^2, 0, 0]; %Kalman Gain K(:,:,n) = pre_M(:,:,n)*H'*(Cw + H*pre_M(:,:,n)*H')^-1; %creating h from previously extracted rx ry and range (R) h = [sqrt(rx^2 + ry^2);atan2(ry,rx)]; %Correction est_s(:,n) = pre_est_s(:,n) + K(:,:,n)*(x(:,n) - h); %Minimum MSE Matrix M(:,:,n) = (I - K(:,:,n)*H)*pre_M(:,:,n); end %% figure plot(s(1,:), s(2,:)) hold on
15. 15. plot(est_s(1,:), est_s(2,:), '--r') hold off legend('Actual track', 'Estimated track') xlabel('X position') ylabel('Y position') title('Particle track') grid on figure plot(x(1,2:end)) legend('Observed range') xlabel('Samples') ylabel('Range') grid on figure plot(x(2,2:end)) legend('Observed bearing') xlabel('Samples') ylabel('Bearing in Radians') grid on