Airplance Guidance System in Simulink

I am developing a simulator for a plane where I want to design a guidance system.

I have a flight path route defined by a set of waypoints, from the origin airport to the destination. I want to take this vector of latitude and longitude coordinates to calculate the heading of the plane.

My problem is that I want the system to recalculate that heading until it reaches the desired waypoint, and when it manages to reach it, change objective to the next waypoint, but I am a bit lost on how to implement that in Simulink.

If someone could help me I will appreciate it!

Answer 1

[Umar]

Hi @Olga, Thank you for posting your question on MATLAB Central — great to see you working on a flight simulator! I'm happy to walk you through how to tackle this. The good news is that you don't need to manually implement the "check if waypoint is reached, then switch to the next one" logic yourself. MATLAB and Simulink already have the right tools to handle this out of the box. Here's what you need to do: CONVERT YOUR LAT/LON WAYPOINTS INTO METERS FIRST Simulink's navigation blocks don't work directly in degrees of latitude and longitude — they work in meters from a local reference point (called a flat-Earth coordinate system). So your first job is to convert your entire waypoint list. You can do this using the lla2flat MATLAB function inside a MATLAB Function block in Simulink. You set your origin airport as the reference point, and it converts every waypoint into [x, y, z] positions in meters. The Aerospace Blockset also has a dedicated "LLA to Flat Earth" block you can wire directly into your diagram. lla2flat function: https://www.mathworks.com/help/aerotbx/ug/lla2flat.html LLA to Flat Earth block: https://www.mathworks.com/help/aeroblks/llatoflatearth.html USE THE WAYPOINT FOLLOWER BLOCK — IT HANDLES SWITCHING AUTOMATICALLY This is the key block you've been looking for. You give it your entire list of waypoints (as an N×3 matrix of [x, y, z] positions) all at once, along with your aircraft's current position and heading. It then continuously outputs a "Desired Course" — the heading your plane should be flying right now. Critically, the block handles waypoint switching automatically. Once your plane gets within a configurable "transition radius" of the current waypoint, the block moves on to the next one on its own. You don't need a loop or any manual counter logic. When the final waypoint is reached, a Status output goes to 1, which you can use to end the simulation or trigger a landing sequence. Waypoint Follower block documentation: https://www.mathworks.com/help/uav/ref/waypointfollower.html ADD A HEADING CONTROLLER TO ACTUALLY FLY THE DESIRED COURSE The Waypoint Follower tells you *where* to point the plane, but you still need a controller to actually steer it there. A simple proportional (P) controller — or a PID if you want more precision — takes the difference between the desired course and the current heading, and outputs a roll command to your aircraft dynamics model. MathWorks has a worked example that puts all of this together for a fixed-wing aircraft, which I'd strongly recommend as your starting point: Tuning Waypoint Following Controller for Fixed-Wing UAV: https://www.mathworks.com/help/uav/ug/tuning-waypoint-follower-for-fixed-wing.html Fourth IF YOU NEED CUSTOM WAYPOINT LOGIC — USE STATEFLOW If you want more control over what happens at each waypoint (for example, changing altitude, logging arrival times, or handling a missed waypoint), you can add a Stateflow chart on top of the Waypoint Follower. Each state in the chart represents a waypoint, and you write a simple transition condition like "distance to current waypoint < 500 metres" to move to the next state. Stateflow is ideal for this kind of mode-switching logic and keeps your diagram clean and readable. Stateflow overview: https://www.mathworks.com/help/stateflow Guidance, Navigation & Control examples: https://www.mathworks.com/help/uav/guidance-navigation-and-control.html REQUIRED TOOLBOXES To follow the approach above you will need: * UAV Toolbox (for the Waypoint Follower block) * Aerospace Blockset (for LLA/flat-Earth conversion and 6DOF dynamics) I hope this gets you unblocked! Don't hesitate to follow up if anything is unclear.

[Olga]

Thanks for the response!

The problem is that I cannot use the UAV Toolbox, with the Waypoint Follower Block it will be much easier for sure, but I need to find a solution without additional Toolboxes.

I have the Aerospace Blockset and Toolbox, do you know any block that would be helpful to solve my problem with those?

Thanks in advance!

[Umar]

Hi @Olga,

No problem at all — good to know about the UAV Toolbox constraint, and the Aerospace Blockset is absolutely enough to get this done. Let me walk you through how to rebuild the same idea using only what you have.

The approach is actually very similar to what I described before, just without the Waypoint Follower block doing the heavy lifting. You'll handle the bearing calculation and waypoint switching yourself inside a MATLAB Function block, which gives you full control anyway.

First, just as before, convert your waypoints from lat/lon degrees into flat-Earth metres using the lla2flat function inside a MATLAB Function block, with your origin airport as the reference point. Nothing changes here from my earlier suggestion.

https://www.mathworks.com/help/aerotbx/ug/lla2flat.html

Now for the core guidance logic — bearing and waypoint switching — put it all inside one MATLAB Function block. You pass in the aircraft's current lat/lon, and it outputs the bearing angle to the current target waypoint. Since you're not modelling wind (so it's bearing, not heading — good catch by the way), you compute this using the standard forward azimuth formula with atan2 and haversine terms. The block also computes the great-circle distance to the current waypoint, and when that drops below a threshold you set (500 metres works well as a starting point), it automatically moves on to the next waypoint.

The trick that replaces the automatic switching the Waypoint Follower block used to handle is a "persistent" variable for the waypoint index. This is a native MATLAB feature that lets a variable hold its value across simulation timesteps — so the counter remembers which waypoint you're on without needing Stateflow or any extra toolbox.

https://www.mathworks.com/help/simulink/ug/implement-matlab-functions-in-simulink-with-matlab-function-blocks.html

If you'd like a native Aerospace Blockset block in the loop for distance, the Calculate Range block takes the aircraft's current flat-Earth [x, y, z] and the target waypoint's [x, y, z] and outputs the scalar distance in metres. You can use it as a cross-check or drive a Relational Operator block with it to trigger the waypoint switch.

https://www.mathworks.com/help/aeroblks/calculaterange.html

For the X-Plane output — lat, lon, alt and the Euler angles — nothing changes from a standard Aerospace Blockset setup. Use the Flat Earth to LLA block to convert your aircraft's flat-Earth position back to geodetic coordinates, and your phi, theta, psi come straight out of the 6DOF (Euler Angles) block.

https://www.mathworks.com/help/aeroblks/flatearthtolla.html https://www.mathworks.com/help/aeroblks/6dofeulerangles.html

Finally, the bearing output from your MATLAB Function block is your desired heading. Subtract your current psi from it to get the heading error, wrap it to [-pi, pi] so the aircraft always turns the short way, and feed it into a PID Controller block to generate your roll or yaw command.

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

So to summarise: one MATLAB Function block with a persistent counter replaces the Waypoint Follower, the Calculate Range block handles native distance output if you need it, and the rest of the pipeline stays exactly as described before. No UAV Toolbox needed anywhere.

Hope that clears it up.

[Olga]

Thank you so much! I am going to try it right away!

[Olga]

Hi @Umar,

I want to do something similar but for computing the flight path angle, could you guide me trought it please?

I have come up with this code, where wp is the waypoint position and trk is the current position of my aircraft

Do you think is it okey?

function gamma = fcn(x_wp,y_wp,z_wp,x_trk,y_trk,z_trk)

dx = x_wp - x_trk;
dy = y_wp - y_trk;
dz = z_wp - z_trk;

gamma = asin(dz/sqrt((dx^2)+(dy^2)+(dz^2)));

end

[Umar]

Hi @Olga, Great progress on this — I wanted to bring everything together in one place so you have a clean reference as you build this out. The overall approach relies on a single MATLAB Function block to handle all your guidance logic without needing the UAV Toolbox. First, you convert your waypoints from latitude/longitude into flat-Earth metres using lla2flat, with your origin airport as the reference point. Inside that same MATLAB Function block, you compute the bearing to the current waypoint using atan2 and haversine terms, track the distance to it, and use a persistent variable to automatically advance to the next waypoint once you come within roughly 500 metres. The persistent variable is what replaces the automatic switching the Waypoint Follower block would have handled — no Stateflow or extra toolbox required. For position output, the Flat Earth to LLA block converts your flat-Earth coordinates back to geodetic latitude, longitude, and altitude, and your roll, pitch, and yaw angles come straight out of the 6DOF (Euler Angles) block. The bearing output from your guidance block becomes your desired heading — subtract the current yaw, wrap the error to the range minus-pi to plus-pi so the aircraft always turns the short way, and feed that into a PID Controller block to generate your roll or yaw command. Regarding the flight path angle function you shared — you are on the right track. The formula structure is correct: flight path angle is indeed the arcsine of the vertical displacement divided by the total 3D range. However, there is one issue you will want to fix before running it. The flat-Earth coordinate system in Aerospace Blockset follows NED convention, meaning the z-axis points downward. A higher-altitude waypoint will have a more negative z value than your aircraft, so dz as you have written it will be negative when you are climbing. That means your gamma will come out negative on a climb and positive on a descent — exactly the wrong way around. The fix is straightforward: negate dz before passing it into asin, like this: gamma = asin(-dz / sqrt(dx^2 + dy^2 + dz^2)); The second thing to add is a guard against division by zero. If your aircraft ever reaches the waypoint precisely, the denominator goes to zero and the simulation will return NaN. A simple range check solves it: range3d = sqrt(dx^2 + dy^2 + dz^2); if range3d < 1.0 gamma = 0; else gamma = asin(-dz / range3d); end Once those two changes are in place, the function is solid and ready to use. Hope this helps tie everything together.

[Olga]

Amaizing, thank you so much!