How to create a oscillating trajectory using the kinematicTrajectory Object?

7 views (last 30 days)
zial_hka
zial_hka on 11 Nov 2021
Answered: Ryan Salvo on 11 Nov 2021
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!

Accepted Answer

Ryan Salvo
Ryan Salvo on 11 Nov 2021
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

More Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!