How is the number of sample points returned by waypointTrajectory determined?

Hello,

I want to simulate some synthetic IMU data with the Sensor Fusion and Tracking Toolbox. For this, I provide a trajectory (position and velocities in 3D, such as a circle or helix) to the system object waypointTrajectory. The outputs (acceleration and angular velocity in local navigation coordinate system) of waypointTrajectory can be fed to the imuSensor object that returns accel and gyro readings.

I have a hard time understanding how the number of sample points is determined by the waypointTrajectory object. If I run

trajectory = waypointTrajectory(waypoints, tVec, 'SampleRate', fs, 'AutoPitch', false, 'AutoBank', false, 'Velocities', velocityAtWaypoints);

where waypoints is a 2514x3 array and tVec = 0:1/fs:tEnd is a vector of length 2514, I would expect that running the code below, would return arrays of dimension 2514 x something. However, the arrays only have 2512 elements. Anyone knows why samples are dropped? If I reduce the sampling rate, to 10 Hz only one sample is dropped (instead of 63 x something, I get 62 x someething).

count = 1;    
while(~isDone(trajectory))
    [position(count,:),orientation(count,:),velocity(count,:),acceleration(count,:),angularVelocity(count,:)] = trajectory();
    count = count + 1;
end

Answer 1

[Greg Dionne]

It's hard to say for certain without trying your specific value for the sample rate, fs, and desired ending time, tEnd, but off-by-one errors are common when using floating-point arithmetic to compare starting and ending times.

When you call waypointTrajectory in a loop, it keeps track of the current time, adding the current value of 1/fs as-you-go. Since neither the current time nor 1/fs may not be exactly representable in floating-point arithmetic, the current time may either undercorrect or overcorrect at each update. waypointTrajectory will continue so long as the current time is less than or equal (in the floating-point sense) than the ending time.

If, by the luck of the draw, the last cumulative time ends up being just shy of the ending time, it will emit a value very close to the ending time and you will receive the expected number of samples. If, however, it overshoots, then the ending time will be exceeded and it won't report the last sample.

If you're simply interested in pose information at a set of time instants, lookupPose() may be of use. You can obtain the results all in one call with specific control of the sample instants. That will avoid accumulation error.

Hope this helps

[Effesian]

Thanks for the explanation! Since I observed that with the higher sampling rate of 400 Hz two samples went missing, floating-point accuracy is most likely not the only culprit. In any case, for my purpose the lookupPose() is exactly what I needed.

[Greg Dionne]

Glad it helped.

Keep in mind that waypointTrajectory is intended to be used as an interpolant (not a lookup table). The idea there is you pick a few sample times of interest and provide the corresponding pose information. You can then set the waypointTrajectory's sample rate to a higher value and it will dutifully interpolate at the finer resolution. So you need not use the same sample rate you used to construct the time of arrivals (the value of fs in tVec=0:1/fs:tEnd) as the sample rate for the trajectory, (trajectory.SampleRate).

If you'd like me to take a look, feel free to post your values for all three of tEnd, fs, and SampleRate. (Hopefully the quantity of tEnd*SampleRate is very close to an integer, otherwise there really isn't much hope of picking up the final end time).

[Revanth Kumar Adireddy]

@Greg Dionne

Dear Greg,

Thanks for your response and valuable information.

I am completely new using MATLAB/SIMULINK. Can you please give some more information about these IMU sensor blocks(about creation of geometric model ) and connections inorder to generate the synthetic/artificial data.

I am stuck at the point of generating accelerometer and gyroscope plots and artificial IMU data. Can you please help me out in solving this issue. As,i am new using matlab i kindly request you to share me the 'error free code' that generates artificial/synthetic data. ([e-mail])

Can please give some suggestions how to create the IMU model uisng matlab.

As a student It would be helpful for me to proceed further and i would appreciate this.

Thanks!!

[Greg Dionne]

Maybe have a look at: https://www.mathworks.com/help/nav/ug/introduction-to-simulating-imu-measurements.html or https://www.mathworks.com/help/nav/ug/estimate-orientation-with-comp-filter-and-imu-data.html

That should get you started.