Borrar filtros
Borrar filtros

Need help with waypoint navigation code

22 visualizaciones (últimos 30 días)
SM
SM el 5 de Oct. de 2024
Comentada: Umar el 25 de Oct. de 2024 a las 2:33
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!
  4 comentarios
Torsten
Torsten el 6 de Oct. de 2024
Editada: Torsten el 6 de Oct. de 2024
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
SM el 6 de Oct. de 2024
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.

Iniciar sesión para comentar.

Respuestas (1)

Umar
Umar el 6 de Oct. de 2024

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.

  6 comentarios
SM
SM el 25 de Oct. de 2024 a las 0:36
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
Umar el 25 de Oct. de 2024 a las 2:33
Hi @SM,
Glad to hear out from you. Please don’t hesitate to ask for help.

Iniciar sesión para comentar.

Productos


Versión

R2024b

Community Treasure Hunt

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

Start Hunting!

Translated by