Commit f2e60c4e by Cung Sang

### Added Constant Acceleration Motion Model for UWB positioning

parent 3e56df07
 ... ... @@ -27,8 +27,19 @@ t2A_4R = [r_t2A0 r_t2A1 r_t2A2 r_t2A3]; % use 4 ranges dimKF = 2; % dimKF = 3; % Init Constant velocity for standard Kalman Fitler [xk, A, Pk, Q, Hkf, R] = initConstVelocity_KF(dimKF); % define the dimension %%%%%%%%%%% Initialization of state parameters %%%%%%%%%%%%% % For Constant velocity Motion Model in standard Kalman Fitler % [xk, A, Pk, Q, Hkf, R] = initConstVelocity_KF(dimKF); % define the dimension % For Constant Acceleration Dynamic/Motion Model [xk, A, Pk, Q, Hkf, R] = initConstAcceleration_KF(dimKF); disp(issymmetric(Q)); d = eig(Q); disp(all(d>0)); disp("The eigen values of process noise (Q) are:"); disp(d); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ... ... @@ -38,7 +49,8 @@ dimKF = 2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Specify an initial guess for the two states initialStateGuess = [2; 1.5; 0; 0]; % the state vector [x, y, vx, vy]; % initialStateGuess = [2; 1.5; 0; 0]; % the state vector [x, y, vx, vy]; initialStateGuess = xk ; %%% Create the extended Kalman filter ekfObject %%% Use function handles to provide the state transition and measurement functions to the ekfObject. ... ... @@ -62,9 +74,8 @@ Q_ekf = Q; ekfObj.ProcessNoise = Q_ekf ; [Nsteps, n] = size(t2A_4R); xCorrectedEKFObj = zeros(Nsteps,4); % Corrected state estimates PCorrectedEKF = zeros(Nsteps,4,4); % Corrected state estimation error covariances e = zeros(Nsteps,4); % Residuals (or innovations) xCorrectedEKFObj = zeros(Nsteps, length(xk)); % Corrected state estimates PCorrectedEKF = zeros(Nsteps, length(xk), length(xk)); % Corrected state estimation error covariances for k=1 : Nsteps ... ... @@ -101,8 +112,8 @@ Q_ukf = Q_ekf; ukf.ProcessNoise = Q_ukf; xCorrectedUKF = zeros(Nsteps,4); % Corrected state estimates PCorrectedUKF = zeros(Nsteps,4,4); % Corrected state estimation error covariances xCorrectedUKF = zeros(Nsteps, length(xk)); % Corrected state estimates PCorrectedUKF = zeros(Nsteps, length(xk), length(xk)); % Corrected state estimation error covariances for k=1:Nsteps ... ...
 ... ... @@ -28,8 +28,19 @@ r_t2A3 = r_t2A3 ./ 1000; t2A_4R = [r_t2A0 r_t2A1 r_t2A2 r_t2A3]; % use 4 ranges dimKF = 2; % % Init Constant velocity for standard Kalman Fitler [xk, A, Pk, Q, Hkf, R] = initConstVelocity_KF(dimKF); % define the dimension %%%%%%%%%%% Initialization of state parameters %%%%%%%%%%%%% % For Constant velocity motion model for standard KF % [xk, A, Pk, Q, Hkf, R] = initConstVelocity_KF(dimKF); % define the dimension % For Constant Acceleration Dynamic/Motion Model [xk, A, Pk, Q, Hkf, R] = initConstAcceleration_KF(dimKF); disp(issymmetric(Q)); d = eig(Q); disp(all(d>0)); disp("The eigen values of process noise (Q) are:"); disp(d); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ... ... @@ -39,7 +50,8 @@ dimKF = 2; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % Specify an initial guess for the two states initialStateGuess = [2; 1.5; 0; 0]; % the state vector [x, y, vx, vy]; % initialStateGuess = [2; 1.5; 0; 0]; % the state vector [x, y, vx, vy]; initialStateGuess = xk; %%% Create the extended Kalman filter ekfObject %%% Use function handles to provide the state transition and measurement functions to the ekfObject. ... ... @@ -59,9 +71,8 @@ ekfObj.ProcessNoise = Q_ekf ; [Nsteps, n] = size(t2A_4R); xCorrectedEKFObj = zeros(Nsteps,4); % Corrected state estimates PCorrectedEKF = zeros(Nsteps,4,4); % Corrected state estimation error covariances e = zeros(Nsteps,4); % Residuals (or innovations) xCorrectedEKFObj = zeros(Nsteps, length(xk)); % Corrected state estimates PCorrectedEKF = zeros(Nsteps, length(xk), length(xk)); % Corrected state estimation error covariances timeVector = 1 : Nsteps; for k=1 : Nsteps ... ... @@ -97,9 +108,8 @@ ukf.MeasurementNoise = R_ukf; Q_ukf = Q_ekf; ukf.ProcessNoise = Q_ukf; xCorrectedUKF = zeros(Nsteps,4); % Corrected state estimates PCorrectedUKF = zeros(Nsteps,4,4); % Corrected state estimation error covariances xCorrectedUKF = zeros(Nsteps, length(xk)); % Corrected state estimates PCorrectedUKF = zeros(Nsteps, length(xk), length(xk)); % Corrected state estimation error covariances for k=1:Nsteps % Incorporate the measurements at time k into the state estimates by ... ...
 ... ... @@ -6,9 +6,6 @@ function yk = citrackMeasurementFcn(xk) % Outputs: % yk - y[k], measurements at time k %#codegen % The tag %#codegen must be included if you wish to generate code with % MATLAB Coder. % Known anchors Positions in 2D at TWB A0_2d = [0, 0]; ... ... @@ -24,7 +21,9 @@ A3_2d = [0, 5.65]; Anc_2D = [A0_2d; A1_2d; A2_2d; A3_2d]; [nAnc, nDim] = size(Anc_2D); yk = zeros(size(xk)); yk = zeros(nAnc, 1); % There are 4 measurements from 4 anchors and a tag % yk = zeros(size(xk)); % TODO: this seems incorrect for CA case for jj = 1 : nAnc % This is the delta range, i.e. the measured minus the best ... ...
 ... ... @@ -24,7 +24,9 @@ A3_2d = [0, 20]; Anc_2D = [A0_2d; A1_2d; A2_2d; A3_2d]; [nAnc, nDim] = size(Anc_2D); yk = zeros(size(xk)); yk = zeros(nAnc, 1); % There are 4 measurements from 4 anchors and a tag % yk = zeros(size(xk)); % TODO: this seems incorrect for CA case for jj = 1 : nAnc % This is the delta range, i.e. the measured minus the best ... ...
 ... ... @@ -6,10 +6,6 @@ function yk = citrackMeasurementFcnSportHall_40x20(xk) % Outputs: % yk - y[k], measurements at time k %#codegen % The tag %#codegen must be included if you wish to generate code with % MATLAB Coder. % Known anchors Positions in 2D at TWB % A0_2d = [0, 0]; % A1_2d = [5.77, 0]; ... ... @@ -30,7 +26,9 @@ A3_2d = [0, 40]; Anc_2D = [A0_2d; A1_2d; A2_2d; A3_2d]; [nAnc, nDim] = size(Anc_2D); yk = zeros(size(xk)); yk = zeros(nAnc, 1); % There are 4 measurements from 4 anchors and a tag % yk = zeros(size(xk)); % TODO: this seems incorrect for CA case for jj = 1 : nAnc % This is the delta range, i.e. the measured minus the best ... ...
 ... ... @@ -30,18 +30,34 @@ A3_2d = [0, 5.65]; Anc_2D = [A0_2d; A1_2d; A2_2d; A3_2d]; [nAnc, nDim] = size(Anc_2D); dhdx = zeros(nAnc, nAnc); state_vec_len = length(xk); dhdx = zeros(nAnc, state_vec_len); ri_0 = zeros(nAnc, 1); for jj = 1 : nAnc % This is the Jacobian Matrix for measurement % dhdx(jj) = sqrt((Anc_2D(jj, 1) - xk(1)).^2 + (Anc_2D(jj, 2) - xk(2)).^2); ri_0(jj) = sqrt((Anc_2D(jj, 1) - xk(1)).^2 + (Anc_2D(jj, 2) - xk(2)).^2); dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; if (state_vec_len == 4) % 2D Constant accelaration model dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; elseif (state_vec_len == 6) dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; dhdx(jj, 5) = 0; % No acceleration data available in the measurement dhdx(jj, 6) = 0; else msg = "Error: 3D state vector is not implemented yet!\n"; error(msg); end end end \ No newline at end of file
 function dhdx = citrackMeasurementJacobianFcnConstAcc(xk) % dhdx = vdpMeasurementJacobianFcn(x) % % Inputs: % x - States x[k] % % Outputs: % dhdx - dh/dx, the Jacobian of citrackMeasurementFcn evaluated at x[k] % %#codegen % The tag %#codegen must be included if you wish to generate code with % MATLAB Coder. % dhdx = [1 0]; % Known anchors Positions in 2D at TWB A0_2d = [0, 0]; A1_2d = [5.77, 0]; A2_2d = [5.55, 5.69]; A3_2d = [0, 5.65]; % Known anchors positions in Sporthall % A0_2d = [0, 0]; % A1_2d = [20, 0]; % A2_2d = [20, 20]; % A3_2d = [0, 20]; Anc_2D = [A0_2d; A1_2d; A2_2d; A3_2d]; [nAnc, nDim] = size(Anc_2D); state_vec_len = length(xk); % dhdx = zeros(nAnc, nAnc); dhdx = zeros(nAnc, state_vec_len); ri_0 = zeros(nAnc, 1); for jj = 1 : nAnc % This is the Jacobian Matrix for measurement % dhdx(jj) = sqrt((Anc_2D(jj, 1) - xk(1)).^2 + (Anc_2D(jj, 2) - xk(2)).^2); ri_0(jj) = sqrt((Anc_2D(jj, 1) - xk(1)).^2 + (Anc_2D(jj, 2) - xk(2)).^2); if (state_vec_len == 4) % 2D Constant accelaration model dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; elseif (state_vec_len == 6) dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; dhdx(jj, 5) = 0; % No acceleration data available in the measurement dhdx(jj, 6) = 0; else msg = "Error: 3D state vector is not implemented yet!\n"; error(msg); end end end \ No newline at end of file
 ... ... @@ -38,16 +38,30 @@ Anc_2D = [A0_2d; A1_2d; A2_2d; A3_2d]; [nAnc, nDim] = size(Anc_2D); dhdx = zeros(nAnc, nAnc); ri_0 = zeros(nAnc, 1); state_vec_len = length(xk); for jj = 1 : nAnc % This is the Jacobian Matrix for measurement % dhdx(jj) = sqrt((Anc_2D(jj, 1) - xk(1)).^2 + (Anc_2D(jj, 2) - xk(2)).^2); ri_0(jj) = sqrt((Anc_2D(jj, 1) - xk(1)).^2 + (Anc_2D(jj, 2) - xk(2)).^2); dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; if (state_vec_len == 4) % 2D Constant accelaration model dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; elseif (state_vec_len == 6) dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; dhdx(jj, 5) = 0; % No acceleration data available in the measurement dhdx(jj, 6) = 0; else msg = "Error: 3D state vector is not implemented yet!\n"; error(msg); end end end \ No newline at end of file
 ... ... @@ -9,13 +9,6 @@ function dhdx = citrackMeasurementJacobianFcnSportHall_40x20(xk) % dhdx - dh/dx, the Jacobian of citrackMeasurementFcn evaluated at x[k] % %#codegen % The tag %#codegen must be included if you wish to generate code with % MATLAB Coder. % dhdx = [1 0]; % Known anchors Positions in 2D at TWB % A0_2d = [0, 0]; % A1_2d = [5.77, 0]; ... ... @@ -38,16 +31,30 @@ Anc_2D = [A0_2d; A1_2d; A2_2d; A3_2d]; [nAnc, nDim] = size(Anc_2D); dhdx = zeros(nAnc, nAnc); ri_0 = zeros(nAnc, 1); state_vec_len = length(xk); for jj = 1 : nAnc % This is the Jacobian Matrix for measurement % dhdx(jj) = sqrt((Anc_2D(jj, 1) - xk(1)).^2 + (Anc_2D(jj, 2) - xk(2)).^2); ri_0(jj) = sqrt((Anc_2D(jj, 1) - xk(1)).^2 + (Anc_2D(jj, 2) - xk(2)).^2); dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; if (state_vec_len == 4) % 2D Constant accelaration model dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; elseif (state_vec_len == 6) dhdx(jj, 1) = (xk(1) - Anc_2D(jj, 1))./ ri_0(jj); dhdx(jj, 2) = (xk(2) - Anc_2D(jj, 2))./ ri_0(jj); dhdx(jj, 3) = 0; % no velocity measurement data dhdx(jj, 4) = 0; dhdx(jj, 5) = 0; % No acceleration data available in the measurement dhdx(jj, 6) = 0; else msg = "Error: 3D state vector is not implemented yet!\n"; error(msg); end end end \ No newline at end of file
 ... ... @@ -22,12 +22,35 @@ function x = citrackStateFcn(x) dt = 0.1; % [s] Sample time % State Transition Matrix A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; % State transition is just a linear in our case % A = [1 0 dt 0; % 0 1 0 dt; % 0 0 1 0; % 0 0 0 1]; %%%%%% State Transition Matrix CV and CA motion models %%%%%% state_vec_len = length(x); if (state_vec_len == 4) % This means 2D state vector for Consta Velocity model % xk = [x, y, vx, vy] A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; elseif (state_vec_len == 6) % This means 2D state vector with const accleration model % xk = [x, y, vx, vy, ax, ay] A = [1 0 dt 0 dt.^2./2 0; 0 1 0 dt 0 dt.^2./2; 0 0 1 0 dt 0; 0 0 0 1 0 dt; 0 0 0 0 1 0; 0 0 0 0 0 1]; else msg = "Error: state vector for 3D is in TODO list and not implemented yet!\n"; error(msg); end % State transition is just a linear fucntion in our case x = A * x; end ... ...
 ... ... @@ -15,13 +15,36 @@ function dfdx = citrackStateJacobianFcn(xk) % dt*(-1-2*xk(1)*xk(2)) 1+dt*(1-xk(1)^2)]; % Since the state is a linear, the Jocobian should not be applied. dt = 0.1; % [s] Sample time dt = 0.1; % [s] Sample time % same as the State Transition Matrix or funcs since it is linear dfdx = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; state_vec_len = length(xk); % % same as the State Transition Matrix or funcs since it is linear % dfdx = [1 0 dt 0; % 0 1 0 dt; % 0 0 1 0; % 0 0 0 1]; if (state_vec_len == 4) % This means 2D state vector for Consta Velocity model % xk = [x, y, vx, vy] dfdx = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; elseif (state_vec_len == 6) % This means 2D state vector with const accleration model % xk = [x, y, vx, vy, ax, ay] dfdx = [1 0 dt 0 dt.^2./2 0; 0 1 0 dt 0 dt.^2./2; 0 0 1 0 dt 0; 0 0 0 1 0 dt; 0 0 0 0 1 0; 0 0 0 0 0 1]; else msg = "Error: state vector for 3D is in TODO list and not implemented yet!\n"; error(msg); end end \ No newline at end of file
 % This implementation is based on the following articles and books: % Ref1: Survey of Maneuvering Target Tracking. Part I: Dynamic Models bz X. Rong Li and et. % Ref2: Estimation with applications to Tracking and Navigation by Z. Bar-Shalom and et al. [chap. 6] % Ref3: Mobile Positioning and Tracking (2nd Edition) by S. Frattasi & F. D. Rosa [sec: 6.4] % % Author: Cung Lian Sang function [Xk, A, Pk, Q, H, R] = initConstAcceleration_KF(dim) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % Constant Accleration Dynamic/Motion Model % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% dt = 0.1; % update rate of the system (10 Hz) % Process noise based on the Decawave datasheet excluding (Z -direction) v_x = 0.01; % the precision (m) of DW1000 in var (10 cm) v_y = 0.01; v_z = 0.01855; % Measurement noise v_xm = 0.015; % the precision (m) of DW1000 in var (10 cm) v_ym = 0.015; v_zm = 0.02855; % 2D implementation in KF if (dim == 2) % Initial guess of State vector: Xk = [ x, y, vx, vy, ax, ay] Xk = [2.5; 2.5; 1.5; 2; 1.0; 1.2]; % a posteriori % State Transition Matrix A = [1 0 dt 0 dt.^2./2 0; 0 1 0 dt 0 dt.^2./2; 0 0 1 0 dt 0; 0 0 0 1 dt 0; 0 0 0 0 1 0; 0 0 0 0 0 1]; % Error Covariance Matrix P (Posterior) % Initial guess just non zero entry. Pk = [ 2 0 0 0 0 0; 0 2 0 0 0 0; 0 0 2 0 0 0; 0 0 0 2 0 0; 0 0 0 0 2 0; 0 0 0 0 0 2]; % Process Noise % accoording to the book "estimation with applications to tracking'', Page % 273, Chapter 6 q_t4 = (dt^4./4); q_t3 = (dt^3./2); q_t2 = (dt^2./2); % Q = [(v_x.*q_t4) 0 (v_x.*q_t3) 0 (v_x.*q_t2) 0; % 0 (v_y.* q_t4) 0 (v_y.*q_t3) 0 (v_x.*q_t2); % (v_x.*q_t3) 0 (v_x.* dt^2) 0 dt 0; % 0 (v_y.*q_t3) 0 (v_y.* dt^2) 0 dt; % (v_x.*q_t2) 0 dt 0 1 0; % 0 (v_x.*q_t2) 0 dt 0 1]; % Approximation of process noise excluding the off-diagonal values % For the purpose of numerical stability/efficiency Q = diag([q_t4 q_t4 dt^2 dt^2 1 1]); % The measurement matrix or Observation model matrix H. % The relation b/w the measurement vector and the state vector H = [ 1 0 0 0 0 0; 0 1 0 0 0 0]; % The measurement noise covariance R R = [ v_xm 0; 0 v_ym]; elseif (dim == 3) % 3D implementation in KF % Initial guess of State vector: Xk = [ x, y, z, vx, vy, vz, ax, ay, az] Xk = [2.5; 2.5; 2.0; 1.5.*0.2; 2.*0.2; 0.5]; % a posteriori % State Transition Matrix A =[1 0 0 dt 0 0; 0 1 0 0 dt 0; 0 0 1 0 0 dt; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1]; % Error Covariance Matrix P (Posterior) % Initial guess just non zero entry. Pk=[2 0 0 0 0 0; 0 2 0 0 0 0; 0 0 2 0 0 0; 0 0 0 2 0 0; 0 0 0 0 2 0; 0 0 0 0 0 2]; % Process Noise % accoording to the book "estimation with applications to tracking'', Page % 273, Chapter 6 q_uD = (dt.^4./4); q_oD = (dt.^3./2); q_lD = (dt.^2); Q =[v_x.*q_uD 0 0 q_oD 0 0; 0 v_y.*q_uD 0 0 q_oD 0; 0 0 v_z.*q_uD 0 0 q_oD; v_x.*q_oD 0 0 q_lD 0 0; 0 v_y.*q_oD 0 0 q_lD 0; 0 0 v_z.*q_oD 0 0 q_lD ]; % The measurement matrix or Observation model matrix H. % The relation b/w the measurement vector and the state vector H=[ 1 0 0 0 0 0; 0 1 0 0 0 0; 0 0 1 0 0 0]; % The measurement noise covariance R R=[ v_xm 0 0; 0 v_ym 0; 0 0 v_zm]; else msg = "Error: undefined motion model for current implementation!\n"; error(msg); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%% End of the Constant Acceleration model %%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% end \ No newline at end of file
 ... ... @@ -58,14 +58,10 @@ if (dim == 2) % (dt.^3./2) 0 (dt.^2) 0; % 0 (dt.^3./2) 0 (dt.^2) ]; % Q = [ v_x.*(dt.^4./4) 0 0 0; % 0 v_y.*(dt.^4./4) 0 0;