79 lines
3.0 KiB
C#
79 lines
3.0 KiB
C#
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
|
|
public class Micromissile : Interceptor {
|
|
[SerializeField]
|
|
private float _navigationGain = 3f; // Typically 3-5
|
|
|
|
private SensorOutput _sensorOutput;
|
|
private Vector3 _accelerationCommand;
|
|
private double _elapsedTime = 0;
|
|
protected override void UpdateMidCourse(double deltaTime) {
|
|
_elapsedTime += deltaTime;
|
|
Vector3 accelerationInput = Vector3.zero;
|
|
if (HasAssignedTarget()) {
|
|
// Update the threat model (assuming we have a threat model)
|
|
// TODO: Implement threat model update logic
|
|
|
|
// Correct the state of the threat model at the sensor frequency
|
|
float sensorUpdatePeriod = 1f / _agentConfig.dynamic_config.sensor_config.frequency;
|
|
if (_elapsedTime >= sensorUpdatePeriod) {
|
|
// TODO: Implement guidance filter to estimate state from sensor output
|
|
// For now, we'll use the threat's actual state
|
|
_sensorOutput = GetComponent<Sensor>().Sense(_target);
|
|
_elapsedTime = 0;
|
|
}
|
|
|
|
// Check whether the threat should be considered a miss
|
|
SensorOutput sensorOutput = GetComponent<Sensor>().Sense(_target);
|
|
if (sensorOutput.velocity.range > 1000f) {
|
|
this.HandleInterceptMiss();
|
|
}
|
|
|
|
// Calculate the acceleration input
|
|
accelerationInput = CalculateAccelerationCommand(_sensorOutput);
|
|
}
|
|
|
|
// Calculate and set the total acceleration
|
|
Vector3 acceleration = CalculateAcceleration(accelerationInput, compensateForGravity: true);
|
|
GetComponent<Rigidbody>().AddForce(acceleration, ForceMode.Acceleration);
|
|
}
|
|
private Vector3 CalculateAccelerationCommand(SensorOutput sensorOutput) {
|
|
// Implement Proportional Navigation guidance law
|
|
Vector3 accelerationCommand = Vector3.zero;
|
|
|
|
// Extract relevant information from sensor output
|
|
float los_rate_az = sensorOutput.velocity.azimuth;
|
|
float los_rate_el = sensorOutput.velocity.elevation;
|
|
float closing_velocity =
|
|
-sensorOutput.velocity
|
|
.range; // Negative because closing velocity is opposite to range rate
|
|
|
|
// Navigation gain (adjust as needed)
|
|
float N = _navigationGain;
|
|
|
|
// Calculate acceleration commands in azimuth and elevation planes
|
|
float acc_az = N * closing_velocity * los_rate_az;
|
|
float acc_el = N * closing_velocity * los_rate_el;
|
|
|
|
// Convert acceleration commands to craft body frame
|
|
accelerationCommand = transform.right * acc_az + transform.up * acc_el;
|
|
|
|
// Clamp the acceleration command to the maximum acceleration
|
|
float maxAcceleration = CalculateMaxAcceleration();
|
|
accelerationCommand = Vector3.ClampMagnitude(accelerationCommand, maxAcceleration);
|
|
|
|
// Update the stored acceleration command for debugging
|
|
_accelerationCommand = accelerationCommand;
|
|
return accelerationCommand;
|
|
}
|
|
|
|
protected override void DrawDebugVectors() {
|
|
base.DrawDebugVectors();
|
|
if (_accelerationCommand != null) {
|
|
Debug.DrawRay(transform.position, _accelerationCommand * 1f, Color.green);
|
|
}
|
|
}
|
|
}
|