How to create a oscillating trajectory using the kinematicTrajectory Object?

Hello there,

currently I'm trying to generate a oscillating trajectory to simulate the movement of a walking person carrying my sensor platform.

I'm trying to generate the trajectory using the "kinematicTrajectory" System Object from the Sensor Fusion/Navigation Toolbox.

The inputs should be the acceleration and angular velocity in the body coordinate system. Outputs should be using the NED coordinate system. I'm currently simulating the oscillation using a simple sine wave function.

I'd like to achieve something like this (top-down-view on the x,y-plane):

black line: "optimal" trajectory without the oscillation

red line: trajectory with oscillation

(please excuse my poor MSPaint drawing...)

Unfortunately my current approach doesn't look similar...

For some reason the trajectory seems to "drift" away without a (for me) visible reason (top-down-view on the x,y-plane):

I'm using this code at the moment:

%%% startup
initSpeed = 1;

samples = 10000;

fs = 10;

ft = 10;
%%%

bodyAcc = zeros(samples, 3);

bodyAngVel = zeros(samples, 3);

for i = 1:samples
    bodyAcc(i, 2) = 5 * sin((i-1) * 2*pi / fs);
end

traj = kinematicTrajectory('SampleRate', ft, ...
    'Velocity', [ initSpeed 0 0 ], ...
    'Orientation', quaternion( [ 0 0 0 ], 'eulerd', 'XYZ', 'frame' ));

[ pos, ~, ~, ~, ~ ] = traj(bodyAcc, bodyAngVel);

figure('Name', 'traj out')
plot3(pos(:,1), pos(:,2), pos(:,3))
xlabel('x (m)')
ylabel('y (m)')
zlabel('z (m)')
title('Position')
axis([0 10 -5 5]) % axis setting
grid on

Maybe someone knows a solution or is having the same problem as me...

Thank you in advance!

Answer 1

[Ryan Salvo]

Hello,

You'll want to generate your sine wave for the body acceleration using your specifed sampling frequency ft and set a non-zero initial y-velocity so that the oscillation occurs:

%%% startup

samples = 1000;
fs = 10;
ft = 10;
initSpeed = 1; %5/(2*pi/fs);
%%%
bodyAcc = zeros(samples, 3);
bodyAngVel = zeros(samples, 3);
for i = 1:samples
    t = (i-1)/ft; % Set "t" based on sampling rate "ft".
    bodyAcc(i, 2) = 5 * sin(t * 2*pi / fs);
end
traj = kinematicTrajectory('SampleRate', ft, ...
    'Velocity', [ initSpeed -5/(2*pi/fs) 0 ], ... % Set y-velocity.
    'Orientation', quaternion( [ 0 0 0 ], 'eulerd', 'XYZ', 'frame' ));
[ pos, ~, ~, ~, ~ ] = traj(bodyAcc, bodyAngVel);
figure('Name', 'traj out')
plot3(pos(:,1), pos(:,2), pos(:,3))
xlabel('x (m)')
ylabel('y (m)')
zlabel('z (m)')
title('Position')
%axis([0 10 -5 5]) % axis setting
grid on
view(2)

Thanks,

Ryan