# Need help with waypoint navigation code

##### 4 Comments

### Answers (1)

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 Comments

### See Also

### Categories

### Community Treasure Hunt

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

Start Hunting!