Commit f2e60c4e authored by Cung Sang's avatar Cung Sang
Browse files

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;