How to create a oscillating trajectory using the kinematicTrajectory Object?
7 visualizzazioni (ultimi 30 giorni)
Mostra commenti meno recenti
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):
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/797289/image.png)
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):
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/797294/image.png)
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!
0 Commenti
Risposta accettata
Ryan Salvo
il 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
0 Commenti
Più risposte (0)
Vedere anche
Categorie
Scopri di più su Trajectory and Scenario Generation in Help Center e File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!