question about using insfilterAsync to simulate a freefall

Hi

I'm new to the sensor fusion. In order to quickly get familiar with it, I decide to simulate something simple, a freefall.

I decede to fuse gyro, accelerometer, Magnetometer and GPS to get the best estimation of the freefall object pose.

In my case, the simulation time is 10 seconds, the frequency of the gyro, accelerometer and magnetometer is 100 Hz, the frequency of the GPS is 1 Hz.

The code can be seen below:

% adding this path is to use a visulization tool
addpath '/Users/yuangao/Documents/MATLAB/Examples/R2019b/shared_positioning/PoseEstimationFromAsynchronousSensorsExample'
%% insfilterAsync trial
fusionfilt = insfilterAsync('ReferenceLocation', [0,0,0]);
% make up some sensor reading to simulate the free fall
% 1000 sample for gyro, accelerometer and magnetometer
acc =repmat([0;0;-9.81],1,1000);
gyro = zeros(3,1000);
magneto = repmat([47.1;-13.9;16.8],1,1000);
t = linspace(1,10,10);
% 100 sample for GPS
GPS_z = 0.5*9.81*t.^2+2*rand(1,10);
GPS = [repmat([0;0],1,10);GPS_z];
GPS_z_vel = 9.81*t+10*rand(1,10);
GPS_vel = [repmat([0;0],1,10);GPS_z_vel];
%% Initialize the State Vector of the |insfilterAsync|
% The |insfilterAsync| tracks the pose states in a 28-element vector.
% The states are:
%
%       States                          Units    Index
%    Orientation (quaternion parts)             1:4  
%    Angular Velocity (XYZ)            rad/s    5:7  
%    Position (NED)                    m        8:10 
%    Velocity (NED)                    m/s      11:13
%    Acceleration (NED)                m/s^2    14:16
%    Accelerometer Bias (XYZ)          m/s^2    17:19
%    Gyroscope Bias (XYZ)              rad/s    20:22
%    Geomagnetic Field Vector (NED)    uT       23:25
%    Magnetometer Bias (XYZ)           uT       26:28
%
% Ground truth is used to help initialize the filter states, so the filter
% converges to good answers quickly.
initstate = zeros(28,1);
initstate(1:4) = [1,0,0,0]'; 
initstate(5:7) = gyro(:,1);
initstate(8:10) = zeros(3,1);
initstate(11:13) = zeros(3,1);
initstate(14:16) = acc(:,1);
initstate(23:25) = magneto(:,1);

% The gyroscope bias initial value estimate is low for the Z-axis. This is
% done to illustrate the effects of fusing the magnetometer in the
% simulation.
initstate(20:22) = deg2rad([3.125 3.125 3.125]); 
fusionfilt.State = initstate;

%% Set the Process Noise Values of the |insfilterAsync|
% The process noise variance describes the uncertainty of the motion model
% the filter uses. 
fusionfilt.QuaternionNoise = 1e-2; 
fusionfilt.AngularVelocityNoise = 100;
fusionfilt.AccelerationNoise = 0.01;
fusionfilt.MagnetometerBiasNoise = 1e-7;
fusionfilt.AccelerometerBiasNoise = 1e-7;
fusionfilt.GyroscopeBiasNoise = 1e-7;

%% Define the Measurement Noise Values Used to Fuse Sensor Data
% Each sensor has some noise in the measurements. These values can
% typically be found on a sensor's datasheet. 
Rmag = 0.4;
Rvel = 0.01;
Racc = 0.0610;
Rgyro = 0.76e-5;
Rpos = 0.034; 

fusionfilt.StateCovariance = diag(1e-3*ones(28,1));
%% get the visulization tool
viewer = PoseViewerWithSwitches(...
        'XPositionLimits', [-300 300], ...
        'YPositionLimits', [-300, 300], ...
        'ZPositionLimits', [-10 1000]);
    
Fs = 100;
j = 1;
% simulation starts here
for i=1:length(acc)
    fusionfilt.predict(1./Fs);
    % fuse accelerometer
    fusionfilt.fuseaccel(acc(:,i)', Racc);
    % fuse gyro
    fusionfilt.fusegyro(gyro(:,i)', Rgyro);       
    % fuse magnetometer
    fusionfilt.fusemag(magneto(:,i)', Rmag);

    % fuse GPS
    if mod(i,100)==0
        fusionfilt.fusegps(GPS(:,j)', Rpos, GPS_vel(:,j)', Rvel);
        j = j+1;
    end
   
    % Plot the pose error
    [p,q] = pose(fusionfilt);
    viewer(p, q, [0 0 0], quaternion([1 0 0 0]));
    i
end

However, I found the value is way off from what I expected. I mean, for a freefall model with 0 initial velocity, after 10 seconds, the position in z should be around 490m.

Here is what I have tried:

1. modify the acceleromoeter reading into [0;0;0]. I think in freefall, the accelerometer should not have reading at all in ideal case.
2. adjust the sensor measure accurancy
3. disable the GPS,gyro and magnetometer, only use accelerometer with high accurancy.

After tried all these, I still can't not simulate a realistic freefall. Do you guys have any suggestion?