Need help with waypoint navigation code

Hello,

I'm having issues getting a simple waypoint navigation code to work. I am simulating the movement of an RC boat that should navigate from its current location and move to waypoints. Current location and waypoints are inputs and the distance to the waypoint and bearing are outputs. Once the last waypoint is reached, I want the boat to circle back to the first waypoint and the loop continues. What I have is as follows:

function [s, bearing] = waypoint_navigation(lat1, lon1, waypoints)
s=zeros(1);
bearing=zeros(1);
R = 6371e3; % Earth's radius in meters
lat1 = deg2rad(lat1);
lon1 = deg2rad(lon1);

for i=1:length(waypoints)
    lat2 = deg2rad(waypoints(i, 1)); 
    lon2 = deg2rad(waypoints(i, 2)); 
    [s(i), bearing(i)] = haversine_distance_and_bearing(lat1, lon1, lat2(i,1), lon2(i,1)); % haversine_distance_and_bearing is a custom function I've created
    
    if s < 5 % Distance threshold
        % Increment the waypoint index to go to the next waypoint
        i = i + 1;
    end
    
    % Loop back to the first waypoint if at the last waypoint
    if i > length(waypoints)
        i = 1;
    end
end
end

Running this results in the error:

Error:An error occurred during simulation and the simulation was terminated

Caused by:

Index exceeds array dimensions. Index value 2 exceeds valid range [1-1] for array 'lat2'.

Error in 'Test_PlantID/Waypoint Navigation' (line 11)

[s(i), bearing(i)] = haversine_distance_and_bearing(lat1, lon1, lat2(i,1), lon2(i,1));

I'd appreciate some help on this, I feel like I'm missing something very basic here. Thanks in advance!

[Torsten]

If you want to return to the first waypoint if i > length(waypoints), your for-loop will never end, will it ?

[SM]

Correct. This is just to simulate a scenario where, in the absence of other commands, the boat continues to circle back and forth between the same points. This is more in the absence of other logic since I am still developing the code.

[Torsten]

If you only want to get rid of the error, replace

[s(i), bearing(i)] = haversine_distance_and_bearing(lat1, lon1, lat2(i,1), lon2(i,1));

by

[s(i), bearing(i)] = haversine_distance_and_bearing(lat1, lon1, lat2, lon2);

Note that "s" is a vector. Thus "if s < 5" means that all elements of the vector s have to be < 5 for the statement to be true. I don't know if this is what you mean.

Returning to waypoint i = 1 makes no difference in the result for s(i) and bearing(i) you will get from "haversine_distance_and_bearing" compared to the first call. So I don't understand the sense behind it.

Changing the loop-index i in a for-loop is really bad programming style, and unexpected results usually follow. Use a while-statement instead.

[SM]

Thanks for your response, Torsten.

So I define “s” as a distance to each waypoint. What I’m trying to do with s<5 is say that if the distance to a waypoint is less than 5 m, start changing course to the next waypoint. I indexed “s” in order to calculate the distance to each waypoint.

Could you please elaborate on using the while loop? Do you mean instead of saying if s<5, use while s<5?

Thanks again for the assistance.

Answer 1

[Umar]

Hi @SM,

To address your query about the waypoint navigation code and its issues, I have broken down the problem, analyzed the code, and providing a comprehensive solution. So, you are trying to simulate the movement of an RC boat that navigates between waypoints based on its current location. The challenge arises from an error during execution due to incorrect indexing, as well as a misunderstanding of how to effectively manage the waypoint navigation logic.

Key Issues Identified

Indexing Errors: The original line lat2(i, 1) and lon2(i, 1) leads to an index out-of-bounds error because lat2 and lon2 are scalar values (not arrays).

Logic for Waypoint Navigation: The use of a for-loop with manual index increment can lead to unpredictable behavior. Instead, a while-loop is more appropriate for continuously checking conditions.

Distance Threshold Logic: The condition if s < 5 was incorrectly implemented; it should check the specific distance to the current waypoint rather than comparing the entire vector. The revised version of your code improves upon these points by:

• Correctly indexing latitude and longitude when calling your custom function.

• Implementing a while-loop that allows continuous navigation until all waypoints are reached.

• Ensuring that distance checks are performed correctly on individual waypoint distances.

Here is the modified version of your waypoint navigation function:

   function [s, bearing] = waypoint_navigation(lat1, lon1, waypoints)
    % Check for sufficient input arguments
    if nargin < 3
        error('Not enough input arguments. Provide latitude, longitude, and 
        waypoints.');
    end

    % Initialize output arrays
    s = zeros(1, length(waypoints)); % Preallocate distance array
    bearing = zeros(1, length(waypoints)); % Preallocate bearing array
    R = 6371e3; % Earth's radius in meters
    lat1 = deg2rad(lat1);
    lon1 = deg2rad(lon1);

    % Initialize waypoint index
    i = 1; 
    while true % Infinite loop to continuously navigate waypoints
        % Get the current waypoint coordinates
        lat2 = deg2rad(waypoints(i, 1)); 
        lon2 = deg2rad(waypoints(i, 2)); 

        % Calculate distance and bearing using the custom function
        [s(i), bearing(i)] = haversine_distance_and_bearing(lat1, lon1, lat2, 
        lon2);

        % Check if the distance to the current waypoint is less than 5 meters
        if s(i) < 5 
            % Move to the next waypoint
            i = i + 1;
            % Loop back to the first waypoint if at the last waypoint
            if i > length(waypoints)
                i = 1; % Reset to the first waypoint
            end
        end

        % Optional: Add a break condition to avoid infinite loop during testing
        if i == 1 && all(s < 5) % Break if all waypoints are reached
            break;
        end

        % Update the current position to the last waypoint reached
        lat1 = lat2;
        lon1 = lon2;
    end

    % Plotting waypoints after navigation loop (optional)
    figure;
    hold on;
    plot(waypoints(:, 2), waypoints(:, 1), 'ro', 'MarkerSize', 10, 'DisplayName',
     'Waypoints'); 
    plot(rad2deg(lon1), rad2deg(lat1), 'bo', 'MarkerSize', 10, 'DisplayName',  
      'Current Position'); 
    xlabel('Longitude');
    ylabel('Latitude');
    title('Waypoint Navigation Path');
    legend;
    grid on;
    hold off;
  end

function [distance, bearing] = haversine_distance_and_bearing(lat1, lon1, lat2,
lon2)
  dlat = lat2 - lat1;
  dlon = lon2 - lon1;

    a = sin(dlat/2)^2 + cos(lat1) * cos(lat2) * sin(dlon/2)^2;
    c = 2 * atan2(sqrt(a), sqrt(1-a));

    R = 6371e3; % Radius of Earth in meters
    distance = R * c; % Distance in meters

    bearing = atan2(sin(dlon) * cos(lat2), cos(lat1) * sin(lat2) - sin(lat1) * 
    cos(lat2) * cos(dlon));
    bearing = rad2deg(bearing); 
    bearing = mod(bearing + 360, 360); 
  end

function test_waypoint_navigation()
  start_lat = 37.7749; 
  start_lon = -122.4194;

    waypoints = [
        37.7750, -122.4183; 
        37.7760, -122.4170; 
        37.7770, -122.4160  
    ];

    [distances, bearings] = waypoint_navigation(start_lat, start_lon, waypoints);

    disp('Distances to waypoints (meters):');
    disp(distances);

    disp('Bearings to waypoints (degrees):');
    disp(bearings);
  end

test_waypoint_navigation();

Please see attached.

In summary, the waypoint_navigation function takes the initial latitude and longitude, along with a matrix of waypoints, to compute the distance and bearing to each waypoint. It first checks for sufficient input arguments and initializes output arrays for distances (s) and bearings. The Earth's radius is defined, and the input coordinates are converted from degrees to radians. An infinite loop is employed to navigate through the waypoints. For each waypoint, the function haversine_distance_and_bearing is called to calculate the distance and bearing from the current position to the waypoint. If the distance to a waypoint is less than 5 meters, the function moves to the next waypoint. The loop continues until all waypoints are reached, at which point a plot visualizes the waypoints and the current position.

The haversine_distance_and_bearing function computes the great-circle distance and bearing using the Haversine formula, ensuring accurate navigation over the Earth's surface.

Finally, the test_waypoint_navigation function demonstrates the usage of the main function with sample coordinates.

If you have any further questions, please let us know.

[SM]

Hi Umar,

Thank you so much for the very detailed and thorough response, I truly appreciate it very much!

I have modified your code and am now trying to use it in a Simulink model (to learn and develop my skills in Simulink). I am relatively new to Simulink so please forgive me if I am missing basic things here. When I try to use the modified code in Simulink, I cannot seem to get the navigation plot correct. What I would also like to do here for testing purposes is implement the break condition you have but this time using Simulink blocks. I understand how to use the stop block but how do you implement "all(s<5)"? For inputs into my MATLAB function block, I use a waypoint index data storage memory block that starts at 1 and resets to 1 once the final destination has been reached. I have attached my Simulink model here for your review.

[Umar]

Hi @SM,

I was not able to open your “Test_PlantID.slx “ since I am using Matlab Mobile version. However, going through documentation provided at the link below,

https://www.mathworks.com/help/simulink/slref/matlabfunction.html

I was able to understand your concerns regarding to implementing your waypoint navigation functionality in Simulink while addressing the issues you've faced, so, based on that I will focus on two primary objectives:

Visualizing the Navigation Path: need to ensure that the waypoints and current position are plotted correctly within your Simulink model.

Implementing Break Conditions: adapt your MATLAB logic to check if all waypoints have been reached using Simulink blocks.

MATLAB Function Block Implementation

Create a MATLAB Function block in your Simulink model. Here is how you can redefine your waypoint_navigation function:

   function [distances, bearings, waypoint_index] = 
   waypoint_navigation(lat1, 
   lon1, waypoints)
       persistent index; % Persistent variable to track the 
       waypoint index

       if isempty(index)
           index = 1; % Initialize the index on first call
       end

       R = 6371e3; % Earth's radius in meters
       lat1 = deg2rad(lat1);
       lon1 = deg2rad(lon1);

       lat2 = deg2rad(waypoints(index, 1)); 
       lon2 = deg2rad(waypoints(index, 2)); 

       % Calculate distance and bearing
       [distances, bearings] = haversine_distance_and_bearing(lat1,
        lon1, lat2, lon2);

       % Check if we have reached the waypoint
       if distances < 5 
           index = index + 1; % Move to next waypoint
           if index > size(waypoints, 1)
               index = 1; % Reset to first waypoint
           end
       end

       waypoint_index = index; % Output current waypoint index
   end

   function [distance, bearing] = 
   haversine_distance_and_bearing(lat1, lon1, 
   lat2, lon2)
       dlat = lat2 - lat1;
       dlon = lon2 - lon1;
       a = sin(dlat/2)^2 + cos(lat1) * cos(lat2) * sin(dlon/2)^2;
       c = 2 * atan2(sqrt(a), sqrt(1-a));
       R = 6371e3; % Radius of Earth in meters
       distance = R * c; % Distance in meters
       bearing = atan2(sin(dlon) * cos(lat2), cos(lat1) * sin(lat2)
       - sin(lat1) * 
       cos(lat2) * cos(dlon));
       bearing = rad2deg(bearing); 
       bearing = mod(bearing + 360, 360); 
   end

This code utilizes a persistent variable index to keep track of which waypoint is currently being approached. The output variables include distances, bearings, and the current waypoint_index.

Simulink Blocks Setup

Input Ports: Create input ports for initial latitude and longitude.

Output Ports: Set up output ports for distances, bearings, and waypoint indices. Use a Scope block or a MATLAB Function block for plotting the waypoints against the current position.

Implementing Break Conditions

To implement the break condition (checking if all distances are less than 5 meters), you can use logical blocks:

• Add a logical block that checks if all outputs from the distance calculation are less than 5 meters.

• Use an "AND" block to combine these conditions into a single output signal that can trigger stopping or resetting behaviors in your model.

Testing Your Model

After setting up your model

• Connect the output of your MATLAB Function block to either a Display or Scope block for visual confirmation.

• Ensure that you run multiple simulations with varying waypoints to validate both the navigation logic and visualization accuracy.

Utilize breakpoints within the MATLAB Function block editor to step through your code and monitor variable values during simulation. If you notice performance issues with large datasets or complex navigation paths, consider optimizing by pre-calculating static values (like converted radians) outside of frequently called loops.

Hope this helps.

If further issues arise or specific scenarios need addressing, please feel free to reach out!

[SM]

Hi Umar,

Thanks a ton for the detailed explanation. I hadn't used a persistent variable in Simulink before - thank you for showing me that, learnt something new!

I incorporated your code but I'm still having difficulty making it work for my case. I added 2 outputs for my case - initial distance and initial bearing. These are for the initial distance and bearing I need to go from my current location to the 1st waypoint. After getting to the 1st waypoint, I want to keep moving between the waypoints which will now exclude my original starting position which was not part of the waypoints. My function block now looks like this:

function [s_initial, bearing_initial, s, bearing, waypoint_index] = waypoint_navigation(lat1, lon1, waypoints)
persistent index; % Persistent variable to track the waypoint index

if isempty(index)
    index = 1; % Initialize index
end

lat2 = (waypoints(:,1));
lon2 = (waypoints(:,2));

[s_initial, bearing_initial] = haversine_distance_and_bearing(lat1, lon1, waypoints(1,1), waypoints(1,2)); % Initial distance and bearing from current location to 1st waypoint.
                                                                                                           % Haversine distance and bearing function takes care of deg2rad.

s = zeros(1);
bearing = zeros(1);

[s(index), bearing(index)] = haversine_distance_and_bearing(waypoints(index,1), waypoints(index,2), waypoints(index+1,1), waypoints(index+1,2));

   if s <= 5
       index = index+1;
       if index+1 > length(waypoints)
           index = 1;
       end
   end   

waypoint_index = index; % Output current waypoint index

end

function [s, bearing] = haversine_distance_and_bearing(lat1, lon1, lat2, lon2)

    R = 6371000; % m

    % Convert degrees to radians
    lat1 = deg2rad(lat1);
    lon1 = deg2rad(lon1);
    lat2 = deg2rad(lat2);
    lon2 = deg2rad(lon2);
    
    % Haversine formula for distance
    dlat = lat2 - lat1;
    dlon = lon2 - lon1;
    a = sin(dlat / 2)^2 + cos(lat1) * cos(lat2) * sin(dlon / 2)^2;
    c = 2 * atan2(sqrt(a), sqrt(1 - a));
    s = R * c;  % Distance in meters
    
    % Formula for initial bearing
    y = sin(dlon) * cos(lat2);
    x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dlon);
    bearing_rad = atan2(y, x);  % Bearing in radians
    bearing = rad2deg(bearing_rad);

end

I am slowly getting there I think. This code shows me the correct distance and bearing from my current location to the 1st waypoint. I also see the distance and bearing from the 1st waypoint to the second, however, I don't seem to move after reaching the 1st waypoint. Your advice would be greatly appreciated. Thanks a ton!

[Umar]

Hi @SM,

Let me break down your code and identify potential issues that could be causing this behavior:

Waypoint Index Update Logic: Your code attempts to update the index variable based on whether the distance to the current waypoint is less than or equal to 5 meters. However, there may be a logical flaw in how you're checking and updating this index.

Distance Calculation: The line where you check if s <= 5 is not correctly indexed because s is initialized as a zeros array. You should be checking the distance to the current waypoint, which is s(index) instead.

Looping Back to Start: Ensure that your indexing logic correctly wraps around when reaching the end of the waypoints array. The current check if index+1 > length(waypoints) might lead to an off-by-one error.

Here is a revised version of your function with corrections:

function [s_initial, bearing_initial, s, bearing, waypoint_index]      =     
waypoint_navigation(lat1, lon1, waypoints)
  persistent index; % Persistent variable to track the waypoint 
  index
  if isempty(index)
      index = 1; % Initialize index
  end

    lat2 = waypoints(:,1);
    lon2 = waypoints(:,2);

    % Initial distance and bearing from current location to 1st 
    waypoint
    [s_initial, bearing_initial] = 
    haversine_distance_and_bearing(lat1, lon1, 
     waypoints(1,1), waypoints(1,2)); 

    % Calculate distance and bearing to next waypoint
    s = zeros(1);
    bearing = zeros(1);

    if index < length(waypoints) % Ensure we do not exceed bounds
        [s(index), bearing(index)] = 
      haversine_distance_and_bearing(waypoints(index,1), 
      waypoints(index,2), 
      waypoints(index+1,1), waypoints(index+1,2));
    else
        [s(index), bearing(index)] = 
     haversine_distance_and_bearing(waypoints(index,1), 
   waypoints(index,2), 
    waypoints(1,1), waypoints(1,2)); % Loop back to start if at 
    last waypoint
    end

    % Check if we have reached the current waypoint
    if s(index) <= 5 % Check distance to current waypoint
        index = index + 1; % Move to next waypoint

        if index > length(waypoints) % Loop back if at last 
     waypoint
            index = 1; 
        end
      end   

      waypoint_index = index; % Output current waypoint index
   end

function [s, bearing] = haversine_distance_and_bearing(lat1, 
lon1, lat2, lon2)
  R = 6371000; % Radius of Earth in meters
  lat1 = deg2rad(lat1);
  lon1 = deg2rad(lon1);
  lat2 = deg2rad(lat2);
  lon2 = deg2rad(lon2);

    dlat = lat2 - lat1;
    dlon = lon2 - lon1;

    a = sin(dlat / 2)^2 + cos(lat1) * cos(lat2) * sin(dlon / 2)^2;
    c = 2 * atan2(sqrt(a), sqrt(1 - a));

    s = R * c;  % Distance in meters

    y = sin(dlon) * cos(lat2);
    x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dlon);

    bearing_rad = atan2(y, x);  
    bearing = rad2deg(bearing_rad); % Convert from radians to 
    degrees
  end

Make sure you test your updated function with various sets of waypoints. This will help confirm that it behaves as expected across different scenarios. Consider adding debug statements (e.g., disp() or logging) within your function to track values of key variables like s, index, and bearings during execution. Also, when integrating with Simulink blocks, ensure that you properly set up input and output ports for your function block.

This adjusted approach should help you move seamlessly from one waypoint to another after reaching your initial target. Keep iterating on your design based on real-world testing!

[SM]

Thank you very much for your help, Umar. I really appreciate it. I haven't had a chance to fix my issues but hopefully I will be able to resolve them soon.

[Umar]

Hi @SM,

Glad to hear out from you. Please don’t hesitate to ask for help.