├── 1126VBAKFQ.pptx ├── Improved_Adaptive_Kalman_Filter_with_Unknown_Process_Noise_Covariance.pdf ├── akf_mhe.m ├── cov_calculator.m ├── curve.m ├── data ├── bag1.mat ├── bag2.mat ├── bag3.mat ├── bag4.mat └── bag5.mat ├── initialize.m ├── kf.m ├── main.m ├── reality.m ├── result.m └── vbakf_q.m /1126VBAKFQ.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prelude404/VBAKF-Q/9e44e2e0a3c6b12faaa9b53df19d99523845b5ed/1126VBAKFQ.pptx -------------------------------------------------------------------------------- /Improved_Adaptive_Kalman_Filter_with_Unknown_Process_Noise_Covariance.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prelude404/VBAKF-Q/9e44e2e0a3c6b12faaa9b53df19d99523845b5ed/Improved_Adaptive_Kalman_Filter_with_Unknown_Process_Noise_Covariance.pdf -------------------------------------------------------------------------------- /akf_mhe.m: -------------------------------------------------------------------------------- 1 | function [x_esti, x_predict, d] = akf_mhe(gtd, imu, uwb, t) 2 | % This VBAKF-Q use state augmentation method but with a sliding window 3 | 4 | %% data preparation 5 | K = length(t); 6 | dt = t(2)-t(1); 7 | x_esti = zeros(8,K); 8 | x_pre = zeros(8,K); 9 | ns = 15; % length of the sliding window 10 | x_esti(1:6,1:ns) = gtd(1:6,1:ns); 11 | x_pre(1:6,1:ns) = gtd(1:6,1:ns); 12 | 13 | x_predict = zeros(8,K); 14 | x_predict(1:6,1:ns) = gtd(1:6,1:ns); 15 | 16 | y0 = zeros(1,K); 17 | y1 = zeros(1,K); 18 | y0(ns+1:K) = 0.5 * (uwb(1:K-ns).^2); 19 | y1(ns+1:K) = 0.5 * (uwb(ns+1:K).*uwb(ns+1:K)); 20 | u = imu; 21 | v = zeros(3,ns+1); 22 | d = zeros(3,K); 23 | y = zeros(1,K); 24 | for i = ns+1:K 25 | for j = 1:ns+1 26 | v(1,j) = dt * trapz(u(1,i-ns:i-ns+j-1)); 27 | v(2,j) = dt * trapz(u(2,i-ns:i-ns+j-1)); 28 | v(3,j) = dt * trapz(u(3,i-ns:i-ns+j-1)); 29 | end 30 | d(1,i) = dt * trapz(v(1,1:ns+1)); 31 | d(2,i) = dt * trapz(v(2,1:ns+1)); 32 | d(3,i) = dt * trapz(v(3,1:ns+1)); 33 | y(i) = y1(i) - y0(i) + 0.5 * (d(1,i)^2 + d(2,i)^2 + d(3,i)^2); 34 | end 35 | 36 | %% x_k = A x_k-1 + B u_k 37 | % Should A and B change when using MHE? 38 | A = [1,0,0,dt,0,0,0,0; 39 | 0,1,0,0,dt,0,0,0; 40 | 0,0,1,0,0,dt,0,0; 41 | 0,0,0, 1,0,0,0,0; 42 | 0,0,0, 0,1,0,0,0; 43 | 0,0,0, 0,0,1,0,0; 44 | 0,0,0, 0,0,0,1,0; 45 | 0,0,0, 0,0,0,0,1]; 46 | 47 | % p0,vo is changing 48 | A1 = [1,0,0,dt,0,0,0,0; 49 | 0,1,0,0,dt,0,0,0; 50 | 0,0,1,0,0,dt,0,0; 51 | 0,0,0, 1,0,0,0,0; 52 | 0,0,0, 0,1,0,0,0; 53 | 0,0,0, 0,0,1,0,0; 54 | 0,0,0, 0,0,0,0,0; 55 | 0,0,0, 0,0,0,0,0]; 56 | 57 | B = [0.5*dt^2,0,0; 58 | 0,0.5*dt^2,0; 59 | 0,0,0.5*dt^2; 60 | dt, 0, 0; 61 | 0, dt, 0; 62 | 0, 0, dt; 63 | 0, 0, 0; 64 | 0, 0, 0]; 65 | 66 | %% Parameter 67 | rho = 1 - 1e-4; 68 | nx = 8; 69 | tau = 2; 70 | R = 1e-2; 71 | P = diag([1e-2,1e-2,1e-2,1e-3,1e-3,1e-3,1e-2,1e-1]); 72 | Q0 = diag([1e-1,1e-1,1e-1,1e-7,1e-7,1e-7,1e-2,1e-4]); 73 | mu = tau + nx +1; 74 | U = tau * Q0; 75 | delta = 1e-6; 76 | N = 10; 77 | 78 | %% VBAKF-Q + MHE + State Augmentation 79 | for i = ns+1:K 80 | % disp(['Step: ',int2str(i)]); 81 | x_predict(:,i) = A1 * x_predict(:,i-1) + B * u(:,i); 82 | x_predict(7,i) = x_predict(1:3,i-ns)'*x_predict(4:6,i-ns); 83 | x_predict(8,i) = x_predict(4:6,i-ns)'*x_predict(4:6,i-ns); 84 | % Prediction 85 | x_pre(:,i) = A1 * x_esti(:,i-1) + B * u(:,i); 86 | x_pre(7,i) = x_esti(1:3,i-ns)'*x_esti(4:6,i-ns); 87 | x_pre(8,i) = x_esti(4:6,i-ns)'*x_esti(4:6,i-ns); 88 | sigma = A * P * A'; 89 | mu = rho * (mu-nx-1) + nx + 1; 90 | U = rho * U; 91 | % Update 92 | H = [d(1,i),d(2,i),d(3,i),0,0,0,(ns * dt),0.5*((ns * dt)^2)]; 93 | theta = x_pre(:,i); 94 | x_former = theta; 95 | for j = 1:N 96 | Aq = U./mu; 97 | % update x 98 | K = Aq * H' * ((H*Aq*H'+R)^(-1)); 99 | x = theta + K * (y(:,i) - H * theta); 100 | P = Aq - K * H * Aq; 101 | % update theta 102 | K1 = sigma * ((sigma + Aq)^(-1)); 103 | theta = x_pre(:,i) + K1 * (x - x_pre(:,i)); 104 | P1 = sigma - K1 * sigma; 105 | % update Q 106 | mu = mu + 1; 107 | U = U + (x-theta)*(x-theta)' + P + P1; 108 | if norm(x - x_former)/norm(x) < delta 109 | break; 110 | end 111 | x_former = x; 112 | end 113 | x_esti(:,i) = x; 114 | % if NaN in x_esti? 115 | TF = isnan(x_esti(:,i)); 116 | if ismember(1,TF) 117 | disp(['Divergence! The step is: ',int2str(i)]); 118 | break; 119 | end 120 | 121 | end 122 | 123 | -------------------------------------------------------------------------------- /cov_calculator.m: -------------------------------------------------------------------------------- 1 | function [Q,R] = cov_calculator(imu, uwb, gtd, t) 2 | % gtd_y 3 | K = length(t); 4 | dt = t(2) - t(1); 5 | gtd_v = zeros(3,K); 6 | gtd_d = zeros(3,K); 7 | gtd_y = zeros(1,K); 8 | gtd_y0 = 0.5 * sum(gtd(1:3,1).^2); 9 | gtd_y1 = 0.5 * sum(gtd(1:3,:).^2); 10 | for i = 1:K 11 | gtd_v(1:3,i) = gtd(4:6,i) - gtd(4:6,1); 12 | gtd_d(1,i) = dt * trapz(gtd_v(1,1:i)); 13 | gtd_d(2,i) = dt * trapz(gtd_v(2,1:i)); 14 | gtd_d(3,i) = dt * trapz(gtd_v(3,1:i)); 15 | 16 | gtd_y(i) = gtd_y1(i) - gtd_y0 + 0.5 * (gtd_d(1,i)^2 + gtd_d(2,i)^2 + gtd_d(3,i)^2); 17 | end 18 | 19 | % y 20 | u = imu; 21 | v = zeros(3,K); 22 | d = zeros(3,K); 23 | y = zeros(1,K); 24 | y0 = 0.5 * (uwb(1).^2); 25 | y1 = 0.5 * (uwb.*uwb); 26 | for i = 1:K 27 | v(1,i) = dt * trapz(u(1,1:i)); 28 | v(2,i) = dt * trapz(u(2,1:i)); 29 | v(3,i) = dt * trapz(u(3,1:i)); 30 | d(1,i) = dt * trapz(v(1,1:i)); 31 | d(2,i) = dt * trapz(v(2,1:i)); 32 | d(3,i) = dt * trapz(v(3,1:i)); 33 | 34 | y(i) = y1(i) - y0 + 0.5 * (d(1,i)^2 + d(2,i)^2 + d(3,i)^2); 35 | end 36 | 37 | % gtd_u 38 | gtd_u = zeros(3,K); 39 | for i = 2:K 40 | gtd_u(1:3,i) = (gtd(4:6,i) - gtd(4:6,i))./dt; 41 | end 42 | 43 | % calculate Q 44 | B = [0.5*dt^2,0,0; 45 | 0,0.5*dt^2,0; 46 | 0,0,0.5*dt^2; 47 | dt, 0, 0; 48 | 0, dt, 0; 49 | 0, 0, dt; 50 | 0, 0, 0; 51 | 0, 0, 0]; 52 | error_u = imu - gtd_u; 53 | Q0 = mean(error_u'.^2); 54 | Q = B * diag(Q0) * B'; 55 | 56 | % calculate R 57 | error_y = y - gtd_y; 58 | R = mean(error_y.^2); 59 | 60 | -------------------------------------------------------------------------------- /curve.m: -------------------------------------------------------------------------------- 1 | function [gtd, u, y, imu, uwb] = curve(imu_noise, uwb_noise, t) 2 | x0 = [50;50;50]; 3 | T = 20; 4 | 5 | p_curve = [x0(1)+200*(cos(2*pi/T*t)-1); 6 | x0(2)+200*sin(1.5*pi/T*t); 7 | x0(3)+200*sin(1*pi/T*t)]; 8 | v_curve = [-200*(2*pi/T)*sin(2*pi/T*t); 9 | 200*(1.5*pi/T)*cos(1.5*pi/T*t); 10 | 200*(1*pi/T)*cos(1*pi/T*t)]; 11 | u_curve = [-200*((2*pi/T)^2)*cos(2*pi/T*t); 12 | -200*((1.5*pi/T)^2)*sin(1.5*pi/T*t); 13 | -200*((1*pi/T)^2)*sin(1*pi/T*t)]; 14 | 15 | gtd(1:3,:) = p_curve; 16 | gtd(4:6,:) = v_curve; 17 | gtd(7,:) = p_curve(:,1)' * v_curve(:,1); 18 | gtd(8,:) = v_curve(:,1)' * v_curve(:,1); 19 | 20 | u = u_curve; 21 | y = sqrt(p_curve(1,:).^2 + p_curve(2,:).^2 + p_curve(3,:).^2); 22 | imu = u + imu_noise; 23 | uwb = y + uwb_noise; 24 | 25 | -------------------------------------------------------------------------------- /data/bag1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prelude404/VBAKF-Q/9e44e2e0a3c6b12faaa9b53df19d99523845b5ed/data/bag1.mat -------------------------------------------------------------------------------- /data/bag2.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prelude404/VBAKF-Q/9e44e2e0a3c6b12faaa9b53df19d99523845b5ed/data/bag2.mat -------------------------------------------------------------------------------- /data/bag3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prelude404/VBAKF-Q/9e44e2e0a3c6b12faaa9b53df19d99523845b5ed/data/bag3.mat -------------------------------------------------------------------------------- /data/bag4.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prelude404/VBAKF-Q/9e44e2e0a3c6b12faaa9b53df19d99523845b5ed/data/bag4.mat -------------------------------------------------------------------------------- /data/bag5.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/prelude404/VBAKF-Q/9e44e2e0a3c6b12faaa9b53df19d99523845b5ed/data/bag5.mat -------------------------------------------------------------------------------- /initialize.m: -------------------------------------------------------------------------------- 1 | function [imu_noise, uwb_noise, K, dt, t] = initialize() 2 | % clc; 3 | clear; 4 | dt = 1/25; 5 | K = 300/dt; 6 | t = 0:dt:K*dt; 7 | t(K+1) = []; 8 | T = 2; 9 | 10 | sigma_u = diag([0.01,0.01,0.01]); 11 | sigma_y = 0.01; 12 | wave_imu = [1;1;1]*(0.55 + 0.45 * sin(2*pi/T*t)); 13 | wave_uwb = 1 + 0.05 * sin(2*pi/(2*T)*t); 14 | 15 | imu_noise = wave_imu .* (sqrt(sigma_u)*randn(3,K)); 16 | uwb_noise = wave_uwb .* (sqrt(sigma_y)*randn(1,K)); 17 | 18 | -------------------------------------------------------------------------------- /kf.m: -------------------------------------------------------------------------------- 1 | function [x_kf] = kf(gtd, imu, uwb, t) 2 | % kf: The classical Kalman filter 3 | % System Model: 4 | % x(k) = A x(k-1) + B u(k) + q 5 | % y(k) = H x(k) + r 6 | 7 | %% Data Preparation: y,u,K,dt 8 | x0 = gtd(1:6,1); 9 | K = length(t); 10 | dt = t(2) - t(1); 11 | u = imu; 12 | v = zeros(3,K); 13 | d = zeros(3,K); 14 | y = zeros(1,K); 15 | y0 = 0.5 * (uwb(1).^2); 16 | y1 = 0.5 * (uwb.*uwb); 17 | for i = 1:K 18 | v(1,i) = dt * trapz(u(1,1:i)); 19 | v(2,i) = dt * trapz(u(2,1:i)); 20 | v(3,i) = dt * trapz(u(3,1:i)); 21 | d(1,i) = dt * trapz(v(1,1:i)); 22 | d(2,i) = dt * trapz(v(2,1:i)); 23 | d(3,i) = dt * trapz(v(3,1:i)); 24 | 25 | y(i) = y1(i) - y0 + 0.5 * (d(1,i)^2 + d(2,i)^2 + d(3,i)^2); 26 | end 27 | 28 | %% x_k = A x_k-1 + B u_k 29 | A = [1,0,0,dt,0,0,0,0; 30 | 0,1,0,0,dt,0,0,0; 31 | 0,0,1,0,0,dt,0,0; 32 | 0,0,0, 1,0,0,0,0; 33 | 0,0,0, 0,1,0,0,0; 34 | 0,0,0, 0,0,1,0,0; 35 | 0,0,0, 0,0,0,1,0; 36 | 0,0,0, 0,0,0,0,1]; 37 | 38 | B = [0.5*dt^2,0,0; 39 | 0,0.5*dt^2,0; 40 | 0,0,0.5*dt^2; 41 | dt, 0, 0; 42 | 0, dt, 0; 43 | 0, 0, dt; 44 | 0, 0, 0; 45 | 0, 0, 0]; 46 | 47 | %% Initialization 48 | x0 = x0 + [0;0;0;0;0;0]; % add initial error 49 | x_esti = zeros(8,K); 50 | x_esti(:,1) = [x0(1:3,1); x0(4:6,1); x0(1:3,1)'*x0(4:6,1); x0(4:6,1)'*x0(4:6,1)]; 51 | x_pre = zeros(8,K); 52 | x_pre(:,1) = x_esti(:,1); 53 | 54 | %% Parameter 55 | P = diag([1e-2,1e-2,1e-2,1e-3,1e-3,1e-3,1e-2,1e-1]); 56 | Q = diag([1e-1,1e-1,1e-1,1e-7,1e-7,1e-7,1e-2,1e-4]); 57 | R = 1e-2; 58 | 59 | %% KF 60 | for i = 2:K 61 | x_pre(:,i) = A * x_esti(:,i-1) + B * u(:,i); 62 | P = A * P * A'+Q; 63 | H = [d(1,i),d(2,i),d(3,i),0,0,0,((i-1)*dt),0.5*(((i-1)*dt)^2)]; 64 | K = P * H' * ((H*P*H'+R)^(-1)); 65 | x_esti(:,i) = x_pre(:,i) + K * (y(i) - H * x_pre(:,i)); 66 | P = (eye(8)-K*H)*P; 67 | end 68 | x_kf = x_esti; 69 | 70 | 71 | -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | close all; 2 | [imu_noise, uwb_noise, K, dt, t] = initialize(); 3 | [gtd, u, y, imu, uwb] = curve(imu_noise, uwb_noise, t); 4 | [x_esti, x_predict] = vbakf_q(gtd, imu, uwb, t); 5 | [error_xyz, error] = result(x_esti, gtd, imu, uwb, t, 1); 6 | disp(['VBAKF-Q ErrorX: ',num2str(error_xyz(1)),' ErrorY: ',num2str(error_xyz(2)),' ErrorZ: ',num2str(error_xyz(3)),' ErrorTotal: ',num2str(error_xyz(4))]); 7 | 8 | % [x_esti1, x_predict1,d] = akf_mhe(gtd, imu, uwb, t); 9 | % [error_xyz1, error1] = result(x_esti1, gtd, imu, uwb, t, 2); 10 | % disp(['AKF-MHE ErrorX: ',num2str(error_xyz1(1)),' ErrorY: ',num2str(error_xyz1(2)),' ErrorZ: ',num2str(error_xyz1(3)),' ErrorTotal: ',num2str(error_xyz1(4))]); 11 | 12 | %% compare with classical KF 13 | % [x_kf] = kf(gtd, imu, uwb, t); 14 | % [error0_xyz,error0] = result(x_kf, gtd, imu, uwb, t, 2); 15 | % disp(['ErrorX0: ',num2str(error0_xyz(1)),' ErrorY0: ',num2str(error0_xyz(2)),' ErrorZ0: ',num2str(error0_xyz(3)),' ErrorTotal0: ',num2str(error0_xyz(4))]); 16 | 17 | %% deal with real data 18 | % [gtd, imu, uwb, K, dt,t] = reality(); 19 | % [x_esti, x_predict] = vbakf_q(gtd, imu, uwb, t); 20 | % 21 | % [error_xyz,error] = result(x_esti, gtd, imu, uwb, t, 1); 22 | % disp(['ErrorX: ',num2str(error_xyz(1)),' ErrorY: ',num2str(error_xyz(2)),' ErrorZ: ',num2str(error_xyz(3)),' ErrorTotal: ',num2str(error_xyz(4))]); 23 | 24 | %% Output 25 | % figure(3) 26 | % plot3(gtd(1,:),gtd(2,:),gtd(3,:),'b-','linewidth',1); 27 | % xlabel('x','FontName','Times New Roman','FontSize',16); 28 | % ylabel('y','FontName','Times New Roman','FontSize',16); 29 | % zlabel('z','FontName','Times New Roman','FontSize',16); 30 | % title('Trajectory','FontName','Times New Roman','FontSize',16); 31 | % grid on; 32 | % 33 | % figure(4) 34 | % axis = 3; 35 | % plot(t,gtd(axis,:),'r-',t,x_esti(axis,:),'m-',t,x_predict(axis,:),'b-','linewidth',1); 36 | % hold on 37 | % h1 = legend('gtd','esti','predict','FontName','Times New Roman','FontSize',12); 38 | % xlabel('Time','FontName','Times New Roman','FontSize',16); 39 | % ylabel('Position','FontName','Times New Roman','FontSize',16); 40 | % set(h1,'Orientation','horizon','Box','on'); 41 | % title('Estimation Result','FontName','Times New Roman','FontSize',16); 42 | % 43 | % figure(5) 44 | % plot(t,gtd(1,:),'r-',t,gtd(2,:),'m-',t,gtd(3,:),'b-','linewidth',1); 45 | % hold on 46 | % plot(t,x_esti(1,:),'r--',t,x_esti(2,:),'m--',t,x_esti(3,:),'b--','linewidth',1); 47 | % h1 = legend('x_{gtd}','y_{gtd}','z_{gtd}','x_{esti}','y_{esti}','z_{esti}','Location','northwest','NumColumns',3,'FontName','Times New Roman','FontSize',12); 48 | % xlabel('Time','FontName','Times New Roman','FontSize',16); 49 | % ylabel('Position','FontName','Times New Roman','FontSize',16); 50 | % set(h1,'Orientation','horizon','Box','on'); 51 | % title('Estimation Result','FontName','Times New Roman','FontSize',16); 52 | % 53 | % figure(6) 54 | % error = gtd - x_esti; 55 | % error_norm = sqrt(error(1,:).^2 + error(2,:).^2 + error(3,:).^2); 56 | % plot(t,error_norm,'k','linewidth',1); 57 | % xlabel('Time','FontName','Times New Roman','FontSize',16); 58 | % ylabel('Error','FontName','Times New Roman','FontSize',16); 59 | % title('Estimation Error','FontName','Times New Roman','FontSize',16); 60 | % % 61 | % figure(7) 62 | % % error_x = sqrt(error(1,:).^2 ); 63 | % % error_y = sqrt(error(2,:).^2 ); 64 | % % error_z = sqrt(error(3,:).^2 ); 65 | % plot(t,error(1,:),'r-',t,error(2,:),'m-',t,error(3,:),'b-','linewidth',1); 66 | % xlabel('Time','FontName','Times New Roman','FontSize',16); 67 | % ylabel('Error','FontName','Times New Roman','FontSize',16); 68 | % title('Estimation Error','FontName','Times New Roman','FontSize',16); 69 | % legend('E_{x}','E_{y}','E_{z}','FontName','Times New Roman','FontSize',12); -------------------------------------------------------------------------------- /reality.m: -------------------------------------------------------------------------------- 1 | function [gtd, imu, uwb, K, dt,t]=reality() 2 | % This function deal with data bag in real flight 3 | 4 | %% load data 5 | data = load('data/bag1.mat'); 6 | % data = load('data/bag2.mat'); 7 | % data = load('data/bag3.mat'); 8 | % data = load('data/bag4.mat'); 9 | % data = load('data/bag5.mat'); 10 | 11 | starti = 175;endi = 605; %bag1 12 | % starti = 175;endi = 735; %bag2 13 | % starti = 115;endi = 740; %bag3 14 | % starti = 210;endi = 1140; %bag4 15 | % starti = 70;endi = 1030; %bag5 16 | 17 | %% store data 18 | % time = data.time; % time 19 | % gtd = data.gtd'; % position from vicon 20 | % gtd(4:6,:) = data.vel'; % velocity from vicon 21 | % uwb = data.uwb; % uwb distance 22 | % imu = data.imu'; % imu data 23 | 24 | t = data.time(starti:endi); 25 | t = t - t(1); 26 | dt = t(2) - t(1); 27 | K = length(t); 28 | uwb = data.uwb(:,starti:endi); 29 | imu = data.imu(starti:endi,:)'; 30 | % imu(3,:) = imu(3,:) - 9.7964; 31 | imu(1,:) = imu(1,:) - 0.11; % eliminate the bias of axis X 32 | imu(2,:) = imu(2,:) + 0.01; % eliminate the bias of axis Y 33 | imu(3,:) = imu(3,:) - 10.08; % eliminate the bias of axis Z 34 | 35 | gtd(1:3,:) = data.gtd(starti:endi,:)'; 36 | gtd(4:6,:) = data.vel(starti:endi,:)'; 37 | gtd(7,:) = gtd(1:3,1)' * gtd(4:6,1); 38 | gtd(8,:) = gtd(4:6,1)' * gtd(4:6,1); 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /result.m: -------------------------------------------------------------------------------- 1 | function [error_xyz,error] = result(x_esti, gtd, imu, uwb, t, fig_num) 2 | % clf; 3 | figure(fig_num) 4 | set(gcf,'Position',[100,20,600,600]); 5 | subplot(4,1,1) 6 | plot3(x_esti(1,:),x_esti(2,:),x_esti(3,:),'b--',gtd(1,:),gtd(2,:),gtd(3,:),'b-','linewidth',1); 7 | legend('x_{esti}','x_{gtd}'); 8 | ylabel('Trajectory'); 9 | title('Result'); 10 | subplot(4,1,2) 11 | plot(t,gtd(1,:),'r-',t,gtd(2,:),'m-',t,gtd(3,:),'b-','linewidth',1); 12 | hold on 13 | plot(t,x_esti(1,:),'r--',t,x_esti(2,:),'m--',t,x_esti(3,:),'b--','linewidth',1); 14 | h1 = legend('x_{gtd}','y_{gtd}','z_{gtd}','x_{esti}','y_{esti}','z_{esti}','Location','northwest'); 15 | ylabel('Position'); 16 | set(h1,'Orientation','horizon','Box','on'); 17 | subplot(4,1,3) 18 | plot(t,gtd(4,:),'r-',t,gtd(5,:),'m-',t,gtd(6,:),'b-','linewidth',1); 19 | hold on 20 | plot(t,x_esti(4,:),'r--',t,x_esti(5,:),'m--',t,x_esti(6,:),'b--','linewidth',1); 21 | h2 = legend('x_{gtd}','y_{gtd}','z_{gtd}','x_{esti}','y_{esti}','z_{esti}','Location','northwest'); 22 | ylabel('Velocity') 23 | set(h2,'Orientation','horizon','Box','on'); 24 | subplot(4,1,4) 25 | error = gtd - x_esti; 26 | error_norm = sqrt(error(1,:).^2 + error(2,:).^2 + error(3,:).^2); 27 | plot(t,error_norm,'k','linewidth',1); 28 | xlabel('Time') 29 | ylabel('Estimation Error') 30 | legend('error') 31 | error_xyz = zeros(1,4); 32 | error_xyz(1) = sqrt(mean(error(1,:).^2)); 33 | error_xyz(2) = sqrt(mean(error(2,:).^2)); 34 | error_xyz(3) = sqrt(mean(error(3,:).^2)); 35 | error_xyz(4) = sqrt(mean(error(1,:).^2 + error(2,:).^2 + error(3,:).^2)); -------------------------------------------------------------------------------- /vbakf_q.m: -------------------------------------------------------------------------------- 1 | function [x_esti,x_predict] = vbakf_q(gtd, imu, uwb, t) 2 | % vbakf_q: The variational Bayesian adaptive Kalman filter with unknown Q. 3 | % System Model: 4 | % x(k) = A x(k-1) + B u(k) + q 5 | % y(k) = H x(k) + r 6 | 7 | %% Data Preparation: y,u,K,dt 8 | x0 = gtd(1:6,1); 9 | K = length(t); 10 | dt = t(2) - t(1); 11 | u = imu; 12 | v = zeros(3,K); 13 | d = zeros(3,K); 14 | y = zeros(1,K); 15 | y0 = 0.5 * (uwb(1).^2); 16 | y1 = 0.5 * (uwb.*uwb); 17 | for i = 1:K 18 | v(1,i) = dt * trapz(u(1,1:i)); 19 | v(2,i) = dt * trapz(u(2,1:i)); 20 | v(3,i) = dt * trapz(u(3,1:i)); 21 | d(1,i) = dt * trapz(v(1,1:i)); 22 | d(2,i) = dt * trapz(v(2,1:i)); 23 | d(3,i) = dt * trapz(v(3,1:i)); 24 | 25 | y(i) = y1(i) - y0 + 0.5 * (d(1,i)^2 + d(2,i)^2 + d(3,i)^2); 26 | end 27 | 28 | %% x_k = A x_k-1 + B u_k 29 | A = [1,0,0,dt,0,0,0,0; 30 | 0,1,0,0,dt,0,0,0; 31 | 0,0,1,0,0,dt,0,0; 32 | 0,0,0, 1,0,0,0,0; 33 | 0,0,0, 0,1,0,0,0; 34 | 0,0,0, 0,0,1,0,0; 35 | 0,0,0, 0,0,0,1,0; 36 | 0,0,0, 0,0,0,0,1]; 37 | 38 | B = [0.5*dt^2,0,0; 39 | 0,0.5*dt^2,0; 40 | 0,0,0.5*dt^2; 41 | dt, 0, 0; 42 | 0, dt, 0; 43 | 0, 0, dt; 44 | 0, 0, 0; 45 | 0, 0, 0]; 46 | 47 | %% Initialization 48 | x0 = x0 + [0;0;0;0;0;0]; % add initial error 49 | x_esti = zeros(8,K); 50 | x_predict = zeros(8,K); 51 | x_esti(:,1) = [x0(1:3,1); x0(4:6,1); x0(1:3,1)'*x0(4:6,1); x0(4:6,1)'*x0(4:6,1)]; 52 | x_predict(:,1) = x_esti(:,1); 53 | x_pre = zeros(8,K); 54 | x_pre(:,1) = x_esti(:,1); 55 | 56 | %% Parameter 57 | rho = 1 - 1e-4; 58 | nx = 8; 59 | tau = 2; 60 | % P = diag([1e-2*[1,1,1],1e-3*[1,1,1],1e-2,1e-1]); 61 | % Q0 = diag([1e-1*[1,1,1],1e-7*[1,1,1],1e-2,1e-4]); 62 | R = 1e-10; 63 | % Q0 = 1e-5 * [0.0001*eye(3),0.0032*eye(3),zeros(3,2); 64 | % 0.0032*eye(3),0.1600*eye(3),zeros(3,2); 65 | % zeros(2,8)]; 66 | 67 | P = diag([1e-2,1e-2,1e-2,1e-3,1e-3,1e-3,1e-2,1e-1]); 68 | Q0 = diag([1e-1,1e-1,1e-1,1e-7,1e-7,1e-7,1e-2,1e-4]); 69 | 70 | 71 | % Q0 = diag([1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-5,1e-6].^2); 72 | % R = 100; 73 | % P = diag([10*ones(1,3) 0.01*ones(1,3) 1 1])*0.001; 74 | 75 | %% Parameter for data bag in real flight 76 | % rho = 1 - 1e-4; 77 | % nx = 8; 78 | % tau = 2; 79 | % P = diag([1e1*[1,1,1],1e-3*[1,1,1],1e-2,1e-5]); 80 | % Q0 = diag([1e1*[1,1,1],1e-7*[1,1,1],1e-4,1e-6])*0.1; 81 | % R = 1e2; 82 | 83 | mu = tau + nx +1; 84 | U = tau * Q0; 85 | delta = 1e-6; 86 | N = 10; 87 | 88 | %% VBAKF-Q 89 | for i = 2:K 90 | % disp(['Step: ',int2str(i)]); 91 | % Prediction 92 | x_predict(:,i) = A * x_predict(:,i-1) + B * u(:,i); 93 | x_pre(:,i) = A * x_esti(:,i-1) + B * u(:,i); 94 | sigma = A * P * A'; 95 | mu = rho * (mu-nx-1) + nx + 1; 96 | U = rho * U; 97 | % Update 98 | H = [d(1,i),d(2,i),d(3,i),0,0,0,((i-1)*dt),0.5*(((i-1)*dt)^2)]; 99 | theta = x_pre(:,i); 100 | x_former = theta; 101 | for j = 1:N 102 | Aq = U./mu; 103 | % update x 104 | K = Aq * H' * ((H*Aq*H'+R)^(-1)); 105 | x = theta + K * (y(:,i) - H * theta); 106 | P = Aq - K * H * Aq; 107 | % update theta 108 | K1 = sigma * ((sigma + Aq)^(-1)); 109 | theta = x_pre(:,i) + K1 * (x - x_pre(:,i)); 110 | P1 = sigma - K1 * sigma; 111 | % update Q 112 | mu = mu + 1; 113 | U = U + (x-theta)*(x-theta)' + P + P1; 114 | if norm(x - x_former)/norm(x) < delta 115 | break; 116 | end 117 | x_former = x; 118 | end 119 | x_esti(:,i) = x; 120 | 121 | % % if NaN in x_esti? 122 | % TF = isnan(x_esti(:,i)); 123 | % if ismember(1,TF) 124 | % disp(['Divergence! The step is: ',int2str(i)]); 125 | % break; 126 | % end 127 | 128 | end 129 | 130 | --------------------------------------------------------------------------------