How is the number of sample points returned by waypointTrajectory determined?
10 views (last 30 days)
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;
[position(count,:),orientation(count,:),velocity(count,:),acceleration(count,:),angularVelocity(count,:)] = trajectory();
count = count + 1;
Greg Dionne on 22 Aug 2022
Edited: Greg Dionne on 22 Aug 2022
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