| | | 1 | | using UnityEngine; |
| | | 2 | | |
| | | 3 | | // The augmented proportional navigation controller applies an input that is proportional to the |
| | | 4 | | // line-of-sight's rotation rate and adds a term to compensate for the target's acceleration. |
| | | 5 | | public class ApnController : PnController { |
| | 18 | 6 | | public ApnController(IAgent agent, float gain) : base(agent, gain) {} |
| | | 7 | | |
| | | 8 | | // Controller-dependent implementation of the control law. |
| | 6 | 9 | | protected override Vector3 Plan(in Transformation relativeTransformation) { |
| | 6 | 10 | | Vector3 accelerationInput = base.Plan(relativeTransformation); |
| | | 11 | | |
| | | 12 | | // Add a feedforward term proportional to the target's acceleration. |
| | 6 | 13 | | Vector3 targetAcceleration = relativeTransformation.Acceleration.Cartesian; |
| | 6 | 14 | | Vector3 normalTargetAcceleration = Vector3.ProjectOnPlane(targetAcceleration, Agent.Forward); |
| | 6 | 15 | | return accelerationInput + Gain / 2 * normalTargetAcceleration; |
| | 6 | 16 | | } |
| | | 17 | | } |