├── Angle of attack and pitch angle using extended kalman filter.m ├── DOA tracking of an UAV using M-estimation Robust Cubature kalman filter.m ├── Direction of angle tracking of an UAV using Cubature Kalman filter.m ├── M-estimation robust cubature kalman filter using bhattacharyya distance.m ├── Pitch angle, raw angle, yaw angle estimation of UAV using Unscented Kalman filter.m ├── README.md └── UAV position tracking using Kalman filter.m /Angle of attack and pitch angle using extended kalman filter.m: -------------------------------------------------------------------------------- 1 | clear;clc 2 | %%UAV using extended kalman 3 | %initial conditions 4 | u=15.264; 5 | to=0; 6 | xu=-0.09426; %stability derevative 7 | xw=5.026; %Angle of Attack Derivative 8 | g=9.81; %gravity 9 | zu=0.07848; %Stability Derivative 10 | zw=-5.31; %Angle of Attack Derivative 11 | uo=0.8305; 12 | mupmwzu=0.2776; 13 | mwpmwzw=-8.855; 14 | mqpmwuo=-8.201; 15 | del_u=[20 22 23 24 26 27]; %forward velocity 16 | del_w=[60 60 64 67 68 69]; %angle of attack 17 | del_q=[2 3 4 2 4 4]; %pitch rate 18 | del_t=[21 23 21 22 32 32]; %pitch angle 19 | del_de=[3 4 3 5 6 7]; %elevation deflection derivations 20 | k=1; 21 | p=[del_u(k);del_w(k);del_q(k);del_t(k)]; 22 | r=[xu xw 0 -g;zu zw uo 0;mupmwzu mwpmwzw mqpmwuo 0;0 0 1 0]; 23 | l=r*p; 24 | s=([0 ;-0.2631; -18.59; 0])*(del_de(k)); 25 | r=l+s; 26 | ls=[r(1);r(2);r(3);r(4)]; %linearised longitudinal uav system 27 | y=[0 0 0 1]*p; 28 | %% short period approximation can be choosen for the simulation... 29 | %of the aircraft elevator transfer function. 30 | del_a=[20 22 23 24 26 27]; %angle of attack 31 | del_q=[2 3 4 2 4 4]; %pitch rate 32 | del_de=[3 4 3 5 6 7]; %elevation deflection derivations 33 | r=[-5.3293 1;-22.2728 -4.5916]; 34 | acc_q=2; 35 | %Process Errors in Process Covaiance Matrix 36 | del_px=20; %initial covariance matrix is choosen intuitively 37 | del_pv=5; 38 | uk=acc_q; 39 | %obsevational error 40 | del_X=25; 41 | del_VX=6; 42 | Xk=[]; 43 | p=[]; 44 | for k=1:1:length(del_a)-1; 45 | w=r*[del_a(k);del_q(k)]; 46 | s=([-0.5269;-32.9831]*del_de(k))+w; 47 | %The Predicted State 48 | A=[-19.92 -145.94 ;1 0 ;0 1 ]; 49 | B=[1;0;0]; 50 | C=[0 329.8 1640.4]; 51 | Xk_=[del_a(k);del_q(k)]; 52 | Xkp1=((A*Xk_)); 53 | Xkp2=((B*uk)); 54 | Xkp=(Xkp1+Xkp2); 55 | p=[p;Xkp]; 56 | %Initialising Process Covariance Matrix 57 | Pk_=[((del_px).^2) 0;0 ((del_pv).^2)]; 58 | Pkp1=((A)*(Pk_)); 59 | Pkp=((Pkp1)*(A')); 60 | %Calculating the Kalman gain 61 | R=[((del_X)^2) 0;0 ((del_VX)^2)]; 62 | H=[1 0 0; 0 1 0]; 63 | K=((Pkp)*H')/((H*Pkp*H')+R); 64 | k=k+1; 65 | Ykm=[del_a(k);del_q(k)]; 66 | C=[1 0;0 1]; 67 | Yk=C*Ykm; 68 | %Calculating the Current State 69 | Xk=[Xk; Xkp + K*(Yk-(H*(Xkp)))]; 70 | %Updating the process covariance matrix 71 | Pk=((eye)-(K*H))*Pkp; 72 | k=k+1; 73 | end 74 | Xkf=[del_a(1)]; 75 | Vkf=[del_q(1)]; 76 | for k=1:3:3*(length(del_a)-1) 77 | Xkf=[Xkf;Xk(k)]; 78 | end 79 | for k=2:3:3*(length(del_q)-1) 80 | Vkf=[Vkf;Xk(k)]; 81 | end 82 | prx=[del_a(1)]; 83 | prv=[del_q(1)]; 84 | for i=1:3:(length(p)-1) 85 | prx=[prx;p(i)]; 86 | end 87 | for i=2:2:(length(p)) 88 | prv=[prv;p(i)]; 89 | end 90 | t=1:1:length(Xkf); 91 | subplot(2,1,1); 92 | plot(t,Xkf,'r--o');hold on; 93 | plot(t,del_a,'b--o'); hold on; 94 | %plot(t,prx,'g--o');hold off; 95 | xlabel('time(sec)'); 96 | ylabel('angle(degrees)'); 97 | legend('Estimation','measurement','location','best'); 98 | title('angle of attack estimation of UAV at each sec'); 99 | subplot (2,1,2); 100 | plot(t,Vkf,'y--o');hold on; 101 | plot(t,del_q,'g--o'); hold on; 102 | %plot(t,prv,'m--o');hold off; 103 | xlabel('time(sec)'); 104 | ylabel('angle(degrees)'); 105 | title('pitch angle estimation of uav at each second'); 106 | legend('Estimation','measurement','location','best'); 107 | suptitle('Unmanned Air Vehicle using Extended kalman filter') 108 | -------------------------------------------------------------------------------- /DOA tracking of an UAV using M-estimation Robust Cubature kalman filter.m: -------------------------------------------------------------------------------- 1 | %% M estimation Robust cubature kalman filter 2 | clear;clc;close all 3 | n=80; 4 | del=1; 5 | R=0.005*eye(4); 6 | Wk=gaussmf(0,R); 7 | q=9*10^-12; 8 | gama=250; 9 | RMSE=[]; 10 | 11 | xk=randi([20 360],1 ,n); 12 | yk=randi([5 10],1 ,n); 13 | xvk=randi([120 140],1 ,n); 14 | yvk=randi([20 40],1 ,n); 15 | xk_=[xk; yk; xvk; yvk]; 16 | 17 | xko=randi([15 45],1 ,n+1); 18 | yko=randi([2 5],1 ,n+1); 19 | xkvo=randi([80 100],1 ,n+1); 20 | ykvo=randi([20 40],1 ,n+1); 21 | 22 | uk1=[]; 23 | uk2=[]; 24 | uk3=[]; 25 | uk4=[]; 26 | for i=1:n 27 | uk_1=(xko(i+1)-xko(i)-del*(xkvo(i))); 28 | uk1=[uk1;uk_1]; 29 | uk_2=(yko(i+1)-yko(i)-del*(ykvo(i))); 30 | uk2=[uk2;uk_2]; 31 | uk_3=xkvo(i+1)-xkvo(i); 32 | uk3=[uk3;uk_3]; 33 | uk_4=ykvo(i+1)-ykvo(i); 34 | uk4=[uk4;uk_4]; 35 | end 36 | ukk=[uk1 uk2 uk3 uk4]'; 37 | 38 | F=[1 0 del 0; 0 1 0 del; 0 0 1 0; 0 0 0 1]; 39 | Q=([((del^3)/3) 0 ((del^2)/2) 0; 0 ((del^3)/3) 0 ((del^2)/2); ((del^2)/2) 0 0 0; 0 ((del^2)/2) 0 0])*q; 40 | Vk=gaussmf(0,Q); 41 | 42 | xk1=F*xk_-ukk+Vk; %dynamic model 43 | 44 | %measurement model 45 | zk=[]; 46 | 47 | for k=1:4 48 | zk1=atan((xk1(k,:)./yk)); 49 | zk=[zk;zk1]; 50 | end 51 | 52 | npts=2*n; 53 | zeeta=sqrt(npts/2)*[eye(4,n/2) -eye(4,n/2)]; %made it n/2 to adjust matrix 54 | 55 | xkc=xk1; 56 | Skk = diag([0.9 pi/6 1 1]); % assumed 57 | pkc = Skk*Skk'; 58 | 59 | xkck=F*xkc+ukk-270; %estimated value 60 | pkck=F*pkc*F'+Q; %q value can be reduced 61 | [U, S, V] = svd(pkck); 62 | skk1 = (0.5*[U;V]*sqrt(S)); 63 | 64 | for i=1:n 65 | xks=sqrt(pkck)*zeeta+xkck; 66 | 67 | zks=[]; 68 | for k=1:4 69 | zks1=atan(xks(k,:)./yk); 70 | zks=[zks;zks1]; 71 | end 72 | zkck=(1/(2*n))*zks; 73 | 74 | pzz=(1/2*n)*(zks-zkck)*(zks-zkck)'+R; 75 | 76 | lamk_b=(inv(pzz)*(zk-zkck))*(zk-zkck)'; %yha transpose hoga shayad end term m 77 | 78 | pkb=inv(((lamk_b*pzz)/gama)-pzz+R); 79 | 80 | pxz=(1/2*n)*(xkck-xks)*(zks-zkck)'+R; 81 | pzz=(1/2*n)*(zks-zkck)*(zks-zkck)'+(pkb); %updated pzz 82 | K=(pxz*(pinv(pzz))); 83 | s=K'; 84 | 85 | xkk=xkck+K*(zk-zkck); 86 | pkk=pkck-K*pzz*s; 87 | 88 | end 89 | for nn=1:n 90 | RMSE1=(sqrt(mean((xkk(1,nn)-xk(nn)).^2))); 91 | RMSE=[RMSE;RMSE1]; 92 | end 93 | figure; 94 | plot(xk,'r--.'); hold on 95 | plot(xkck(1,:),'b--o');hold on 96 | plot(xkk(1,:),'g--o'); hold off 97 | ylabel('direction of angle'); 98 | xlabel('sampling interval(sec)'); 99 | legend('measurement','estimation','predicted','location','best'); 100 | suptitle('direction of angle tracking using M-Est Robust cubature kalman filter'); 101 | figure; 102 | plot(RMSE); 103 | ylabel('RMSE '); 104 | xlabel('sampling interval'); 105 | title('RMSE value for each sample'); 106 | -------------------------------------------------------------------------------- /Direction of angle tracking of an UAV using Cubature Kalman filter.m: -------------------------------------------------------------------------------- 1 | %%cubature kalman filter 2 | clear;clc 3 | n=40; 4 | del=1; 5 | R=0.005*eye(4); 6 | Wk=gaussmf(0,R); 7 | q=9*10^-12; 8 | 9 | xk=randi([20 360],1 ,n); 10 | yk=randi([5 10],1 ,n); 11 | xvk=randi([120 140],1 ,n); 12 | yvk=randi([20 40],1 ,n); 13 | xk_=[xk; yk; xvk; yvk]; 14 | 15 | xko=randi([15 45],1 ,n+1); 16 | yko=randi([2 5],1 ,n+1); 17 | xkvo=randi([80 100],1 ,n+1); 18 | ykvo=randi([20 40],1 ,n+1); 19 | 20 | uk1=[]; 21 | uk2=[]; 22 | uk3=[]; 23 | uk4=[]; 24 | for i=1:n 25 | uk_1=(xko(i+1)-xko(i)-del*(xkvo(i))); 26 | uk1=[uk1;uk_1]; 27 | uk_2=(yko(i+1)-yko(i)-del*(ykvo(i))); 28 | uk2=[uk2;uk_2]; 29 | uk_3=xkvo(i+1)-xkvo(i); 30 | uk3=[uk3;uk_3]; 31 | uk_4=ykvo(i+1)-ykvo(i); 32 | uk4=[uk4;uk_4]; 33 | end 34 | ukk=[uk1 uk2 uk3 uk4]'; 35 | 36 | F=[1 0 del 0; 0 1 0 del; 0 0 1 0; 0 0 0 1]; 37 | Q=([((del^3)/3) 0 ((del^2)/2) 0; 0 ((del^3)/3) 0 ((del^2)/2); ((del^2)/2) 0 0 0; 0 ((del^2)/2) 0 0])*q; 38 | Vk=gaussmf(0,Q); 39 | 40 | xk1=F*xk_-ukk+Vk; %dynamic model 41 | 42 | %measurement model 43 | zk=[]; 44 | 45 | for k=1:4 46 | zk1=atan((xk1(k,:)./yk)); 47 | zk=[zk;zk1]; 48 | end 49 | 50 | npts=2*n; 51 | zeeta=sqrt(npts/2)*[eye(4,n/2) -eye(4,n/2)]; %made it n/2 to adjust matrix 52 | 53 | xkc=xk1; 54 | Skk = diag([0.9 pi/6 1 1]); % assumed 55 | pkc = Skk*Skk'; 56 | 57 | xkck=F*xkc+ukk-270; %estimated value 58 | pkck=F*pkc*F'+Q; %q value can be reduced 59 | [U, S, V] = svd(pkck); 60 | skk1 = (0.5*[U;V]*sqrt(S)); 61 | 62 | for i=1:n 63 | xks=sqrt(pkck)*zeeta+xkck; 64 | 65 | zks=[]; 66 | for k=1:4 67 | zks1=atan(xks(k,:)./yk); 68 | zks=[zks;zks1]; 69 | end 70 | zkck=(1/(2*n))*zks; 71 | 72 | pzz=(1/2*n)*(zks-zkck)*(zks-zkck)'+R; 73 | 74 | pxz=(1/2*n)*(xkck-xks)*(zks-zkck)'+R; 75 | 76 | K=(pxz*(pinv(pzz))); 77 | s=K'; 78 | 79 | xkk=xkck+K*(zk-zkck); 80 | pkk=pkck-K*pzz*s; 81 | end 82 | RMSE=sqrt(mean((xk-xkk(1,:)).^2)); 83 | 84 | plot(xk,'r--.'); hold on 85 | plot(xkck(1,:),'b--.');hold on 86 | plot(xkk(1,:),'g--o'); hold off 87 | ylabel('direction of angle(degrees)'); 88 | xlabel('sampling interval(sec)'); 89 | legend('measurement','estimation','predicted','location','best'); 90 | suptitle('direction of angle tracking using cubature kalman filter'); 91 | 92 | 93 | 94 | 95 | 96 | 97 | -------------------------------------------------------------------------------- /M-estimation robust cubature kalman filter using bhattacharyya distance.m: -------------------------------------------------------------------------------- 1 | %% M estimation Robust cubature kalman filter using Bhattacharya distance 2 | clear;clc 3 | n=40; 4 | del=1; 5 | R=0.005*eye(4); 6 | Wk=gaussmf(0,R); 7 | q=9*10^-12; 8 | gama=255; 9 | RMSE=[]; 10 | xk=randi([20 360],1 ,n); 11 | yk=randi([5 10],1 ,n); 12 | xvk=randi([120 140],1 ,n); 13 | yvk=randi([20 40],1 ,n); 14 | xk_=[xk; yk; xvk; yvk]; 15 | 16 | xko=randi([15 45],1 ,n+1); 17 | yko=randi([2 5],1 ,n+1); 18 | xkvo=randi([80 100],1 ,n+1); 19 | ykvo=randi([20 40],1 ,n+1); 20 | 21 | uk1=[]; 22 | uk2=[]; 23 | uk3=[]; 24 | uk4=[]; 25 | for i=1:n 26 | uk_1=(xko(i+1)-xko(i)-del*(xkvo(i))); 27 | uk1=[uk1;uk_1]; 28 | uk_2=(yko(i+1)-yko(i)-del*(ykvo(i))); 29 | uk2=[uk2;uk_2]; 30 | uk_3=xkvo(i+1)-xkvo(i); 31 | uk3=[uk3;uk_3]; 32 | uk_4=ykvo(i+1)-ykvo(i); 33 | uk4=[uk4;uk_4]; 34 | end 35 | ukk=[uk1 uk2 uk3 uk4]'; 36 | 37 | F=[1 0 del 0; 0 1 0 del; 0 0 1 0; 0 0 0 1]; 38 | Q=([((del^3)/3) 0 ((del^2)/2) 0; 0 ((del^3)/3) 0 ((del^2)/2); ((del^2)/2) 0 0 0; 0 ((del^2)/2) 0 0])*q; 39 | Vk=gaussmf(0,Q); 40 | 41 | xk1=F*xk_-ukk+Vk; %dynamic model 42 | 43 | %measurement model 44 | zk=[]; 45 | 46 | for k=1:4 47 | zk1=atan((xk1(k,:)./yk)); 48 | zk=[zk;zk1]; 49 | end 50 | 51 | npts=2*n; 52 | zeeta=sqrt(npts/2)*[eye(4,n/2) -eye(4,n/2)]; %made it n/2 to adjust matrix 53 | 54 | xkc=xk1; 55 | Skk = diag([0.9 pi/6 1 1]); % assumed 56 | pkc = Skk*Skk'; 57 | 58 | xkck=F*xkc+ukk-270; %estimated value 59 | pkck=F*pkc*F'+Q; %q value can be reduced 60 | [U, S, V] = svd(pkck); 61 | skk1 = (0.5*[U;V]*sqrt(S)); 62 | 63 | for i=1:n 64 | xks=sqrt(pkck)*zeeta+xkck; 65 | 66 | zks=[]; 67 | for k=1:4 68 | zks1=atan(xks(k,:)./yk); 69 | zks=[zks;zks1]; 70 | end 71 | zkck=(1/(2*n))*zks; 72 | 73 | pzz=(1/2*n)*(zks-zkck)*(zks-zkck)'+R; 74 | 75 | 76 | Db=bhattacharyya(zk,zkck); 77 | 78 | pkb=inv(((Db*pzz)/gama)-pzz+R); 79 | 80 | pxz=(1/2*n)*(xkck-xks)*(zks-zkck)'+R; 81 | pzz=(1/2*n)*(zks-zkck)*(zks-zkck)'+(pkb); %updated pzz 82 | K=(pxz*(inv(pzz))); 83 | s=K'; 84 | 85 | xkk=xkck+K*(zk-zkck); 86 | pkk=pkck-K*pzz*s; 87 | end 88 | 89 | for nn=1:n 90 | RMSE1=(sqrt(mean((xkk(1,nn)-xk(nn)).^2))); 91 | RMSE=[RMSE;RMSE1]; 92 | end 93 | 94 | figure; 95 | plot(xk,'r--.'); hold on 96 | plot(xkck(1,:),'b--o');hold on 97 | plot(xkk(1,:),'g--o'); hold off 98 | ylabel('direction of angle(degrees)'); 99 | xlabel('sampling interval(sec)'); 100 | legend('measurement','estimation','predicted','location','best'); 101 | suptitle('direction of angle tracking by cubature kalman filter using bhattacharyya distance '); 102 | 103 | figure; 104 | plot(RMSE); 105 | ylabel('RMSE '); 106 | xlabel('sampling interval'); 107 | title('RMSE value for each sample'); 108 | 109 | 110 | 111 | 112 | 113 | -------------------------------------------------------------------------------- /Pitch angle, raw angle, yaw angle estimation of UAV using Unscented Kalman filter.m: -------------------------------------------------------------------------------- 1 | clear;clc 2 | N=80; 3 | theeta=randi([0 180],1,N); 4 | phi=randi([0 360],1,N); 5 | shi=randi([0 90],1,N); 6 | P=60; %roll rate 7 | Q=68; %pitch rate 8 | R=90; %yaw rate 9 | 10 | theeta_=(theeta(1)/2); 11 | phi_=(phi(1)/2); 12 | shi_=(shi(1)/2); 13 | q0=(cos(phi_)*cos(theeta_)*cos(shi_))+(sin(phi_)*sin(theeta_)*sin(shi_)); 14 | q1=(sin(phi_)*cos(theeta_)*cos(shi_))-(cos(phi_)*sin(theeta_)*sin(shi_)); 15 | q2=(cos(phi_)*sin(theeta_)*cos(shi_))+(sin(phi_)*cos(theeta_)*sin(shi_)); 16 | q3=(cos(phi_)*cos(theeta_)*sin(shi_))-(sin(phi_)*sin(theeta_)*cos(shi_)); 17 | q=[q0; q1; q2; q3]; 18 | q_=[(([0 -P -Q -R;P 0 R -Q;Q -R 0 P;R Q -P 0])/2)*q]; %linearised quarternion state 19 | 20 | T = 0.1; % Sampling time 21 | % Step 1: Define UT Scaling parameters and weight vectors 22 | L = 4; % Size of state vector 23 | alpha = 1; % Primary scaling parameter 24 | beta = 2; % Secondary scaling parameter (Gaussian assumption) 25 | kappa = 0; % Tertiary scaling parameter 26 | lambda = alpha^2*(L+kappa) - L; 27 | wm = ones(2*L + 1,1)*1/(2*(L+lambda)); 28 | wc = wm; 29 | wm(1) = lambda/(lambda+L); 30 | wc(1) = lambda/(lambda+L) + 1 - alpha^2 + beta; 31 | % Step 2: Define noise assumptions 32 | Q = diag([0 0 4 4]); 33 | R = diag([1 1]); 34 | % Step 3: Initialize state and covariance 35 | x = zeros(4, N); % Initialize size of state estimate for all k 36 | x(:,1) = [0; 0; 50; 50]; % Set initial state estimate 37 | P0 = eye(4,4); % Set initial error covariance 38 | % Simulation Only: Calculate true state trajectory for comparison 39 | % Also calculate measurement vector 40 | w = sqrt(Q)*randn(4, N); % Generate random process noise (from assumed Q) 41 | v = sqrt(R)*randn(2, N); % Generate random measurement noise (from assumed R) 42 | xt = zeros(4, N); % Initialize size of true state for all k 43 | xt(:,1) = [0; 0; 50; 50] + sqrt(P0)*randn(4,1); % Set true initial state 44 | yt = zeros(2, N); % Initialize size of output vector for all k 45 | for k = 2:N 46 | xt(:,k) = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1]*xt(:,k-1) + w(:,k-1); 47 | yt(:,k) = [sqrt((xt(1,k)-q_(1))^2 + (xt(2,k)-q_(2))^2); ... 48 | sqrt((xt(1,k)-q_(3))^2 + (xt(2,k)-q_(4))^2)] + v(:,k); 49 | end 50 | P = P0; % Set first value of P to the initial P0 51 | for k = 2:N 52 | % Step 1: Generating the sigma-points 53 | sP = chol(P,'lower'); % Calculate square root of error covariance 54 | % chi_p = "chi previous" = chi(k-1) 55 | chi_p = [x(:,k-1), x(:,k-1)*ones(1,L)+sqrt(L+lambda)*sP,x(:,k-1)*ones(1,L)-sqrt(L+lambda)*sP]; 56 | % Step 2: Prediction Transformation 57 | % Propagate each sigma-point through prediction 58 | % chi_m = "chi minus" = chi(k|k-1) 59 | chi_m = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1]*chi_p; 60 | x_m = chi_m*wm; % Calculate mean of predicted state 61 | % Calculate covariance of predicted state 62 | P_m = Q; 63 | for i = 1:2*L+1 64 | P_m = P_m + wc(i)*(chi_m(:,i) - x_m)*(chi_m(:,i) - x_m)'; 65 | end 66 | % Step 3: Observation Transformation 67 | % Propagate each sigma-point through observation 68 | psi_m = [sqrt((chi_m(1,:)-q_(1)).^2 + (chi_m(2,:)-q_(2)).^2); ... 69 | sqrt((chi_m(1,:)-q_(3)).^2 + (chi_m(2,:)-q_(4)).^2)]; 70 | y_m = psi_m*wm; % Calculate mean of predicted output 71 | % Calculate covariance of predicted output 72 | % and cross-covariance between state and output 73 | Pyy = R; 74 | Pxy = zeros(L,2); 75 | for i = 1:2*L+1 76 | Pyy = Pyy + wc(i)*(psi_m(:,i) - y_m)*(psi_m(:,i) - y_m)'; 77 | Pxy = Pxy + wc(i)*(chi_m(:,i) - x_m)*(psi_m(:,i) - y_m)'; 78 | end 79 | % Step 4: Measurement Update 80 | K = Pxy/Pyy; % Calculate Kalman gain 81 | x(:,k) = x_m + K*(yt(:,k) - y_m); % Update state estimate 82 | P = P_m - K*Pyy*K'; % Update covariance estimate 83 | end 84 | % Display results 85 | figure(1); 86 | t = T*(1:N); 87 | subplot(4,1,1) 88 | plot(t,x(1,:),'b--.'); hold on 89 | plot(t,xt(1,:),'r--.') ;hold off 90 | legend('estimate','measurement','location','best'); 91 | ylabel('1st quaternion'); 92 | 93 | 94 | subplot(4,1,2); 95 | plot(t,x(2,:),'b--.'); hold on 96 | plot(t,xt(2,:),'r--.'); hold off 97 | legend('estimate','measurement','location','best'); 98 | ylabel('2nd quaternion'); 99 | subplot(4,1,3) 100 | plot(t,x(3,:),'b--.'); hold on 101 | plot(t,xt(3,:),'r--.'); hold off 102 | legend('estimate','measurement','location','best'); 103 | ylabel('3rd quaternion'); 104 | 105 | subplot(4,1,4) 106 | plot(t,x(4,:),'b--.'); hold on 107 | plot(t,xt(4,:),'r--.'); hold off 108 | legend('estimate','measurement','location','best'); 109 | ylabel('4th quaternion'); 110 | 111 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kalman-filter-and-its-extensions 2 | Kalman filter, Extended Kalman filter, Unscented kalman filter, Cubature Kalman filter, M-estimation Robust cubature kalman filter 3 | implementation for various linear and non linear systems like UAV position tracking, UAV angle of attack and pitch angle tracking, 4 | UAV direction of angle tracking etc. 5 | -------------------------------------------------------------------------------- /UAV position tracking using Kalman filter.m: -------------------------------------------------------------------------------- 1 | %%Target Tracking 2 | clc; 3 | clear all; 4 | %plane is flying but we will be considering its position only in the x 5 | %direction to make it a 2-d case. 6 | %initial state 7 | xo=4000; 8 | vox=280; 9 | %Observations 10 | X=[4000 4260 4550 4860 5410 5600 5990 6400 6790 7000]; 11 | V=[280 282 285 286 290 292 294 296 299 302]; 12 | %Process Errors in Process Covaiance Matrix 13 | del_px=20; %initial covariance matrix is choosen intuitively 14 | del_pv=5; 15 | %initial conditions 16 | acc_x=2; 17 | del_t=1; 18 | vx=280; 19 | del_x=25; %uncertainity in the measurement 20 | %Observation Error 21 | del_X=25; 22 | del_VX=6; 23 | Xk=[]; 24 | %The Predicted State 25 | A=[1 del_t;0 1]; 26 | B=[(0.5*((del_t).^2));del_t]; 27 | p=[]; 28 | for k=1:1:length(X)-1; 29 | Xk_= [X(k);V(k)]; 30 | uk=acc_x; 31 | Xkp1=((A*Xk_)); 32 | Xkp2=((B*uk)); 33 | Xkp=(Xkp1+Xkp2);%this is our first estimation 34 | p=[p;Xkp]; 35 | %Initialising Process Covariance Matrix 36 | Pk_=[((del_px).^2) 0;0 ((del_pv).^2)]; 37 | %Predicted Process Covariance Matrix 38 | Pkp1=((A)*(Pk_)); 39 | Pkp2=((Pkp1)*(A')); 40 | pkp=(Pkp2-[0 Pkp2(2);Pkp2(3) 0]); %since the 2nd and 3rd term are not imp. 41 | %Calculating the Kalman gain 42 | R=[((del_X)^2) 0;0 ((del_VX)^2)]; 43 | H=[1 0 ; 0 1]; 44 | K=((pkp)*H')/((H*pkp*H')+R); 45 | %The New Observation 46 | k=k+1; 47 | Ykm=[X(k);V(k)]; 48 | C=[1 0;0 1]; 49 | Yk=C*Ykm; 50 | %Calculating the Current State 51 | Xk=[Xk; Xkp + K*(Yk-(H*(Xkp)))]; 52 | %Updating the process covariance matrix 53 | Pk1=((eye)-(K*H))*pkp; 54 | pk=(Pk1-[0 Pk1(3);Pk1(2) 0]); 55 | k=k+1; 56 | end 57 | Xkf=[X(1)]; 58 | Vkf=[V(1)]; 59 | for k=1:2:(length(Xk)-1) 60 | Xkf=[Xkf;Xk(k)]; 61 | end 62 | for k=2:2:(length(Xk)) 63 | Vkf=[Vkf;Xk(k)]; 64 | end 65 | prx=[X(1)]; 66 | prv=[V(1)]; 67 | for i=1:2:(length(p)-1) 68 | prx=[prx;p(i)]; 69 | end 70 | for i=2:2:(length(p)) 71 | prv=[prv;p(i)]; 72 | end 73 | 74 | Y1=[1200 1300 1480 1590 1700 1800 1990 2090 2200 2300]; 75 | V1=[20 22 23 25 27 29 32 34 37 40]; 76 | %Process Errors in Process Covaiance Matrix 77 | del_py=20; %initial covariance matrix is choosen intuitively 78 | del_pv1=5; 79 | %initial conditions 80 | acc_y=2; 81 | del_t=1; 82 | vy=280; 83 | del_y=25; %uncertainity in the measurement 84 | %Observation Error 85 | del_Y=25; 86 | del_VY=6; 87 | Yk=[]; 88 | %The Predicted State 89 | A=[1 del_t;0 1]; 90 | B=[(0.5*((del_t).^2));del_t]; 91 | q=[]; 92 | for k=1:1:length(Y1)-1; 93 | Yk_= [Y1(k);V1(k)]; 94 | uk2=acc_x; 95 | Ykp1=((A*Yk_)); 96 | Ykp2=((B*uk2)); 97 | Ykp=(Ykp1+Ykp2);%this is our first estimation 98 | q=[q;Ykp]; 99 | %Initialising Process Covariance Matrix 100 | Pk2_=[((del_py).^2) 0;0 ((del_pv1).^2)]; 101 | %Predicted Process Covariance Matrix 102 | Pkp12=((A)*(Pk_)); 103 | Pkp22=((Pkp1)*(A')); 104 | pkp2=(Pkp22-[0 Pkp22(2);Pkp22(3) 0]); %since the 2nd and 3rd term are not imp. 105 | %Calculating the Kalman gain 106 | R1=[((del_Y)^2) 0;0 ((del_VY)^2)]; 107 | H=[1 0 ; 0 1]; 108 | K=((pkp2)*H')/((H*pkp2*H')+R1); 109 | %The New Observation 110 | k=k+1; 111 | Ykm1=[Y1(k);V1(k)]; 112 | C=[1 0;0 1]; 113 | Yk1=C*Ykm1; 114 | %Calculating the Current State 115 | Yk=[Yk; Ykp + K*(Yk1-(H*(Ykp)))]; 116 | %Updating the process covariance matrix 117 | Pk12=((eye)-(K*H))*pkp2; 118 | pk2=(Pk12-[0 Pk12(3);Pk12(2) 0]); 119 | k=k+1; 120 | end 121 | Ykf=[Y1(1)]; 122 | Vkf1=[V1(1)]; 123 | for k=1:2:(length(Yk)-1) 124 | Ykf=[Ykf;Yk(k)]; 125 | end 126 | for k=2:2:(length(Xk)) 127 | Vkf1=[Vkf1;Yk(k)]; 128 | end 129 | pry=[Y1(1)]; 130 | prv1=[V1(1)]; 131 | for i=1:2:(length(q)-1) 132 | pry=[pry;q(i)]; 133 | end 134 | for i=2:2:(length(q)) 135 | prv1=[prv1;q(i)]; 136 | end 137 | E=[]; 138 | for k=1:1:(length(X)) 139 | E1=abs(((Xkf(k)-X(k))/Xkf(k))*100); 140 | E=[E;E1]; 141 | end 142 | 143 | t=1:1:(length(X)); 144 | subplot(3,1,1); 145 | plot(X,Y1,'r--o');hold on; 146 | plot(Xkf,Ykf,'g--o'); hold on; 147 | plot(prx,pry,'b--o');hold off; 148 | xlabel('position in x direction (m)'); 149 | ylabel('pos in y dir (m)'); 150 | legend('measurement','estimation','predicted','location','best'); 151 | title('Position estimation of plane in x-y plane'); 152 | subplot(3,1,2); 153 | plot(t,V,'m--o'); hold on; 154 | plot(t,Vkf,'b--o'); hold on; 155 | plot(t,prv,'c--o'); hold off; 156 | xlabel('time(per sec)'); 157 | ylabel('vel(m/s) in x dir'); 158 | legend('measurement','estimation','predicted','location','best'); 159 | %title('velocity estimation of plane in x direction'); 160 | %subplot(4,1,3); 161 | %plot(t,V1,'m--o'); hold on; 162 | %plot(t,Vkf1,'b--o'); hold on; 163 | %plot(t,prv1,'c--o'); hold off; 164 | %xlabel('time(per sec)'); 165 | %ylabel('velocity in y direction'); 166 | %legend('measurement','estimation','predicted','location','best'); 167 | %title('velocity estimation of plane in y direction'); 168 | subplot(3,1,3); 169 | plot(t,E,'m--o'); hold on; 170 | ylim([-2 5]); 171 | xlabel('time(per sec)'); 172 | ylabel('Error(in %)'); 173 | legend('Error','location','best'); 174 | %title('Error in accurate tracking'); 175 | 176 | suptitle('Tracking of a plane') 177 | 178 | 179 | --------------------------------------------------------------------------------