wheelEncoderAckermann is generating negative ticks, why and how to fix this?

So I need to generate wheel encoder measurements. I am using the block wheelEncoderAckermann and it's not supposed to give me negative ticks but it does

I am adding my whole code:

clc 
clear

lla0 = [48.151719 17.072900 152];

waypoints = [0 0 0;
             30 0 0;
             30 -15 0;
             0 -15 0;
             0 0 0];
speed = [2 0 0;
         2 0 0;
        -2 0 0;
        -2 0 0;
         2 0 0];
t = cumsum([0 30/2 15*pi/2/2 30/2 15*pi/2/2]');
eulerAngs = [0 0 0;
             0 0 0;
           180 0 0;
           180 0 0;
             0 0 0];
% smoothTrajectory(v,waypoints,speed);
quats = quaternion(eulerAngs,'eulerd','ZYX','frame');
fs = 200;

traj = waypointTrajectory(waypoints,'SampleRate',fs, ...
        'Velocities',speed,...
        'TimeOfArrival',t,...
        'Orientation',quats);

[pos, orient, vel, acc, angvel] = traj();

i = 1;
spf = traj.SamplesPerFrame;
while ~isDone(traj)
    idx = (i+1):(i+spf);
    [pos(idx,:), orient(idx,:), ...
        vel(idx,:), acc(idx,:), angvel(idx,:)] = traj();
    i = i+spf;
end
figure
plot(pos(:,1),pos(:,2), waypoints(:,1),waypoints(:,2), '--o')
xlabel('X (m)')
ylabel('Y (m)')
zlabel('Z (m)')
legend({'Trajectory', 'Waypoints'})
axis equal

% figure
% plot(waypoints(:,1),waypoints(:,2),'-o')
% xlabel('X (m)')
% ylabel('Y (m)')
% title('Vehicle Position Waypoints')


% Initialize the random number generator used to simulate sensor noise.
rng('default');

%IMU
mountingLocationIMU = [1 2 3];
mountingAnglesIMU = [0 0 0];
imuFs = 100;    %sample rate IMU
% Convert orientation offset from Euler angles to quaternion.
orientVeh2IMU = quaternion(mountingAnglesIMU,'eulerd','ZYX','frame');

imu = imuSensor('accel-gyro', ...
    'ReferenceFrame', 'ENU', 'SampleRate', imuFs);

% Accelerometer parameters
imu.Accelerometer.MeasurementRange =  19.6133;
imu.Accelerometer.Resolution = 0.0023928;
imu.Accelerometer.NoiseDensity = 0.0012356;
%imu.Accelerometer.RandomWalk = 0.002; % toto som pridal'

% Gyroscope parameters
imu.Gyroscope.MeasurementRange = deg2rad(250);
imu.Gyroscope.Resolution = deg2rad(0.0625);
imu.Gyroscope.NoiseDensity = deg2rad(0.025);
%imu.Gyroscope.RandomWalk = deg2rad(0.015); % toto som pridal

%GPS
mountingLocationGPS = [1 2 3];
mountingAnglesGPS = [50 40 30];
gpsFs = 10;     %sample rate GPS
% Convert orientation offset from Euler angles to quaternion.
orientVeh2GPS = quaternion(mountingAnglesGPS,'eulerd','ZYX','frame');

% GPS parameters
gps = gpsSensor('UpdateRate', gpsFs, 'ReferenceFrame', 'ENU');
gps.ReferenceLocation = lla0;
gps.DecayFactor = 0.5;                % Random walk noise parameter
gps.HorizontalPositionAccuracy = 1.0;
gps.VerticalPositionAccuracy =  1.0;
gps.VelocityAccuracy = 0.1;

%ODOMETRY
WheelBase = 5; % razvor - rozdiel medzi prednou a zadnou napravou
TrackWidth = [1.5 1.5]; % rozchod - rozdiel medzi lavym a pravym kolesom
encoder = wheelEncoderAckermann('TrackWidth',TrackWidth,...
    'WheelBase',WheelBase,'SampleRate',imuFs);


% IMU readings.
IMU_accel = [];
IMU_gyro = [];
% Wheel encoder readings.
Odometry_ticks = [];
% GPS readings.
GPS_lla = [];
GPS_vel = [];
ticks = [];

% Define the rate of the GPS and IMU compared to the simulation rate.
simSamplesPerGPS = fs/gpsFs;
simSamplesPerIMU = fs/imuFs;

idx = 1;
for sampleIdx = 1:length(pos)
    % simulacia IMU
    if (mod(idx, simSamplesPerIMU) == 0)
    posVeh = pos(idx,:);
    orientVeh = orient(idx,:);
    velVeh = vel(idx,:);
    accVeh = acc(idx,:);
    angvelVeh = angvel(idx,:);
    
    % Convert motion quantities from vehicle frame to IMU frame.
    [posIMU,orientIMU,velIMU,accIMU,angvelIMU] = transformMotion( ...
        mountingLocationIMU,orientVeh2IMU, ...
        posVeh,orientVeh,velVeh,accVeh,angvelVeh);
    [IMU_accel(end+1,:), IMU_gyro(end+1,:)] = imu(accIMU,angvelIMU,orientIMU); 

    % simulacia odometry
    [ticks(end+1,:)] = encoder(velVeh, angvelVeh, orientVeh); 
    end

    % simulacia GPS
    if (mod(idx, simSamplesPerGPS) == 0)
       [posGPS,orientGPS,velGPS,accGPS,angvelGPS] = transformMotion(...
           mountingLocationGPS, orientVeh2GPS,...
           posVeh,orientVeh,velVeh,accVeh,angvelVeh);
       [GPS_lla(end+1,:), GPS_vel(end+1,:)] = gps(posGPS,velGPS);
    end

    idx = idx + 1;            
end

figure
plot(ticks(:,1),'b')
hold on
plot(ticks(:,2),'r')
hold on
plot(ticks(:,3))
b.FaceColor = "#EDB120";
hold on
plot(ticks(:,4))
b.FaceColor = "#7E2F8E";
ylabel('Wheel Ticks')
xlabel('Sample')
title('Wheel Encoder')
legend('ticks BL','ticks BR','ticks FL','ticks FR','Location','best')
grid on

figure
plot(IMU_accel(:,1),'r')
hold on
plot(IMU_accel(:,2),'g')
hold on
plot(IMU_accel(:,3),'b')
ylabel('m/s^2')
xlabel('Sample')
title('Accelerometer')
legend('acc X','acc Y','acc Z','Location','best')
grid on

figure
plot(IMU_gyro(:,1),'r')
hold on
plot(IMU_gyro(:,2),'g')
hold on
plot(IMU_gyro(:,3),'b')
ylabel('rad/s')
xlabel('Sample')
title('Gyroscope')
legend('rot X (roll)','rot Y (pitch)','rot Z (yaw)','Location','best')
grid on

figure
geoplot(GPS_lla(:,1),GPS_lla(:,2))
title('GPS Position')

figure
plot(GPS_vel(:,1),'r')
hold on
plot(GPS_vel(:,2),'g')
hold on
plot(GPS_vel(:,3),'b')
ylabel('m/s')
title('GPS Velocity')
legend('vel X','vel Y','vel Z','Location','best')
grid on

Answer 1

[Rasmita]

Hi Patrik,

It is my understanding that, you want to know why you are getting negative values of ‘Wheel Ticks’. But there may be problem with the input data you have taken like ‘speed’, ‘waypoints’ and ‘eulerAngs’. So kindly check the input data.

For more information on these inputs see the example given in following documentation:

https://www.mathworks.com/help/nav/ref/wheelencoderackermann-system-object.html

Hope this helps!

Regards,

Rasmita