Add adaptive extrapolation mode, quadratic regression, CSV analysis tool
Anti-recoil prediction improvements: - New ARM_AdaptiveExtrapolation mode: velocity variance-based confidence with separate pos/aim tracking and low-speed protection - New ARM_WeightedLinearRegression mode: preserves original simple linear fit - ARM_WeightedRegression upgraded to quadratic (y=a+bt+ct²) with linear/quadratic blend and velocity-reversal clamping - ExtrapolationDamping parameter (all modes): exp decay on extrapolated velocity - CSV recording (RecordPredictionCSV) for offline parameter tuning - Python analysis tool (Tools/analyze_antirecoil.py) to find optimal AdaptiveSensitivity from recorded data Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
9407d8a556
commit
fa257fb87b
365
Tools/analyze_antirecoil.py
Normal file
365
Tools/analyze_antirecoil.py
Normal file
@ -0,0 +1,365 @@
|
||||
"""
|
||||
Anti-Recoil Prediction Analyzer
|
||||
================================
|
||||
Reads CSV files recorded by the EBBarrel CSV recording feature and finds
|
||||
optimal AdaptiveSensitivity and scaling factor for the Adaptive Extrapolation mode.
|
||||
|
||||
Usage:
|
||||
python analyze_antirecoil.py <path_to_csv>
|
||||
python analyze_antirecoil.py <path_to_csv> --plot (requires matplotlib)
|
||||
|
||||
The script:
|
||||
1. Loads per-frame data (real position/aim vs predicted position/aim)
|
||||
2. Computes prediction error at each frame
|
||||
3. Simulates different AdaptiveSensitivity values offline
|
||||
4. Finds the value that minimizes total prediction error
|
||||
5. Reports recommended parameters
|
||||
"""
|
||||
|
||||
import csv
|
||||
import sys
|
||||
import math
|
||||
import os
|
||||
from dataclasses import dataclass
|
||||
from typing import List, Tuple
|
||||
|
||||
|
||||
@dataclass
|
||||
class Frame:
|
||||
timestamp: float
|
||||
real_pos: Tuple[float, float, float]
|
||||
real_aim: Tuple[float, float, float]
|
||||
pred_pos: Tuple[float, float, float]
|
||||
pred_aim: Tuple[float, float, float]
|
||||
safe_count: int
|
||||
buffer_count: int
|
||||
extrap_time: float
|
||||
|
||||
|
||||
def load_csv(path: str) -> List[Frame]:
|
||||
frames = []
|
||||
with open(path, 'r') as f:
|
||||
reader = csv.DictReader(f)
|
||||
for row in reader:
|
||||
frames.append(Frame(
|
||||
timestamp=float(row['Timestamp']),
|
||||
real_pos=(float(row['RealPosX']), float(row['RealPosY']), float(row['RealPosZ'])),
|
||||
real_aim=(float(row['RealAimX']), float(row['RealAimY']), float(row['RealAimZ'])),
|
||||
pred_pos=(float(row['PredPosX']), float(row['PredPosY']), float(row['PredPosZ'])),
|
||||
pred_aim=(float(row['PredAimX']), float(row['PredAimY']), float(row['PredAimZ'])),
|
||||
safe_count=int(row['SafeCount']),
|
||||
buffer_count=int(row['BufferCount']),
|
||||
extrap_time=float(row['ExtrapolationTime']),
|
||||
))
|
||||
return frames
|
||||
|
||||
|
||||
def vec_dist(a, b):
|
||||
return math.sqrt(sum((ai - bi) ** 2 for ai, bi in zip(a, b)))
|
||||
|
||||
|
||||
def vec_sub(a, b):
|
||||
return tuple(ai - bi for ai, bi in zip(a, b))
|
||||
|
||||
|
||||
def vec_add(a, b):
|
||||
return tuple(ai + bi for ai, bi in zip(a, b))
|
||||
|
||||
|
||||
def vec_scale(a, s):
|
||||
return tuple(ai * s for ai in a)
|
||||
|
||||
|
||||
def vec_len(a):
|
||||
return math.sqrt(sum(ai * ai for ai in a))
|
||||
|
||||
|
||||
def vec_normalize(a):
|
||||
l = vec_len(a)
|
||||
if l < 1e-10:
|
||||
return (0, 0, 0)
|
||||
return tuple(ai / l for ai in a)
|
||||
|
||||
|
||||
def angle_between(a, b):
|
||||
"""Angle in degrees between two direction vectors."""
|
||||
dot = sum(ai * bi for ai, bi in zip(a, b))
|
||||
dot = max(-1.0, min(1.0, dot))
|
||||
return math.degrees(math.acos(dot))
|
||||
|
||||
|
||||
def compute_prediction_error(frames: List[Frame]) -> dict:
|
||||
"""Compute error between predicted and actual (real) positions/aims."""
|
||||
pos_errors = []
|
||||
aim_errors = []
|
||||
|
||||
for f in frames:
|
||||
pos_err = vec_dist(f.pred_pos, f.real_pos)
|
||||
pos_errors.append(pos_err)
|
||||
|
||||
aim_a = vec_normalize(f.pred_aim)
|
||||
aim_b = vec_normalize(f.real_aim)
|
||||
if vec_len(aim_a) > 0.5 and vec_len(aim_b) > 0.5:
|
||||
aim_err = angle_between(aim_a, aim_b)
|
||||
aim_errors.append(aim_err)
|
||||
|
||||
if not pos_errors:
|
||||
return {'pos_mean': 0, 'pos_p95': 0, 'pos_max': 0, 'aim_mean': 0, 'aim_p95': 0, 'aim_max': 0}
|
||||
|
||||
pos_errors.sort()
|
||||
aim_errors.sort()
|
||||
|
||||
p95_idx_pos = int(len(pos_errors) * 0.95)
|
||||
p95_idx_aim = int(len(aim_errors) * 0.95) if aim_errors else 0
|
||||
|
||||
return {
|
||||
'pos_mean': sum(pos_errors) / len(pos_errors),
|
||||
'pos_p95': pos_errors[min(p95_idx_pos, len(pos_errors) - 1)],
|
||||
'pos_max': pos_errors[-1],
|
||||
'aim_mean': sum(aim_errors) / len(aim_errors) if aim_errors else 0,
|
||||
'aim_p95': aim_errors[min(p95_idx_aim, len(aim_errors) - 1)] if aim_errors else 0,
|
||||
'aim_max': aim_errors[-1] if aim_errors else 0,
|
||||
}
|
||||
|
||||
|
||||
def simulate_adaptive(frames: List[Frame], sensitivity: float) -> List[Tuple[float, float]]:
|
||||
"""
|
||||
Simulate the adaptive extrapolation offline with given sensitivity.
|
||||
Uses separate pos/aim confidences with relative variance (matching C++ code).
|
||||
Returns list of (position_error, aim_error) per frame.
|
||||
"""
|
||||
pos_errors = []
|
||||
aim_errors = []
|
||||
|
||||
# Sliding window for velocity estimation
|
||||
window_size = 12
|
||||
|
||||
SMALL = 1e-10
|
||||
|
||||
for i in range(window_size + 1, len(frames) - 1):
|
||||
# Build velocity pairs from recent real positions and aims
|
||||
pos_velocities = []
|
||||
aim_velocities = []
|
||||
weights = []
|
||||
for j in range(1, min(window_size, i)):
|
||||
dt = frames[i - j].timestamp - frames[i - j - 1].timestamp
|
||||
if dt > 1e-6:
|
||||
pos_vel = vec_scale(vec_sub(frames[i - j].real_pos, frames[i - j - 1].real_pos), 1.0 / dt)
|
||||
aim_vel = vec_scale(vec_sub(frames[i - j].real_aim, frames[i - j - 1].real_aim), 1.0 / dt)
|
||||
pos_velocities.append(pos_vel)
|
||||
aim_velocities.append(aim_vel)
|
||||
w = (window_size - j) ** 2
|
||||
weights.append(w)
|
||||
|
||||
if len(pos_velocities) < 2:
|
||||
continue
|
||||
|
||||
total_w = sum(weights)
|
||||
|
||||
# Weighted average velocity (position)
|
||||
avg_pos_vel = (0.0, 0.0, 0.0)
|
||||
avg_aim_vel = (0.0, 0.0, 0.0)
|
||||
for pv, av, w in zip(pos_velocities, aim_velocities, weights):
|
||||
avg_pos_vel = vec_add(avg_pos_vel, vec_scale(pv, w / total_w))
|
||||
avg_aim_vel = vec_add(avg_aim_vel, vec_scale(av, w / total_w))
|
||||
|
||||
# Weighted variance (position and aim separately)
|
||||
pos_var_sum = 0.0
|
||||
aim_var_sum = 0.0
|
||||
for pv, av, w in zip(pos_velocities, aim_velocities, weights):
|
||||
pd = vec_sub(pv, avg_pos_vel)
|
||||
ad = vec_sub(av, avg_aim_vel)
|
||||
pos_var_sum += (pd[0]**2 + pd[1]**2 + pd[2]**2) * w
|
||||
aim_var_sum += (ad[0]**2 + ad[1]**2 + ad[2]**2) * w
|
||||
pos_variance = pos_var_sum / total_w
|
||||
aim_variance = aim_var_sum / total_w
|
||||
|
||||
# Separate confidences with RELATIVE variance (matching C++ code)
|
||||
pos_speed2 = sum(v**2 for v in avg_pos_vel)
|
||||
aim_speed2 = sum(v**2 for v in avg_aim_vel)
|
||||
|
||||
# Extrapolation time
|
||||
extrap_dt = frames[i].extrap_time if frames[i].extrap_time > 0 else 0.011
|
||||
|
||||
pos_confidence = 1.0
|
||||
if pos_speed2 > SMALL:
|
||||
pos_rel_var = pos_variance / pos_speed2
|
||||
pos_confidence = 1.0 / (1.0 + pos_rel_var * sensitivity)
|
||||
# Low-speed protection: if extrapolation offset < 1cm, blend confidence toward 1
|
||||
pos_extrap_offset = math.sqrt(pos_speed2) * extrap_dt
|
||||
pos_offset_significance = max(0.0, min(pos_extrap_offset / 1.0, 1.0)) # 1cm threshold
|
||||
pos_confidence = 1.0 + (pos_confidence - 1.0) * pos_offset_significance
|
||||
|
||||
aim_confidence = 1.0
|
||||
if aim_speed2 > SMALL:
|
||||
aim_rel_var = aim_variance / aim_speed2
|
||||
aim_confidence = 1.0 / (1.0 + aim_rel_var * sensitivity)
|
||||
# Low-speed protection: if aim extrapolation offset is tiny, blend toward 1
|
||||
aim_extrap_offset = math.sqrt(aim_speed2) * extrap_dt
|
||||
aim_offset_significance = max(0.0, min(aim_extrap_offset / 0.01, 1.0)) # 0.01 unit threshold
|
||||
aim_confidence = 1.0 + (aim_confidence - 1.0) * aim_offset_significance
|
||||
|
||||
# Predict
|
||||
pred_pos = vec_add(frames[i - 1].real_pos, vec_scale(avg_pos_vel, extrap_dt * pos_confidence))
|
||||
pred_aim_raw = vec_add(frames[i - 1].real_aim, vec_scale(avg_aim_vel, extrap_dt * aim_confidence))
|
||||
pred_aim = vec_normalize(pred_aim_raw)
|
||||
|
||||
# Errors
|
||||
pos_errors.append(vec_dist(pred_pos, frames[i].real_pos))
|
||||
real_aim_n = vec_normalize(frames[i].real_aim)
|
||||
if vec_len(pred_aim) > 0.5 and vec_len(real_aim_n) > 0.5:
|
||||
aim_errors.append(angle_between(pred_aim, real_aim_n))
|
||||
|
||||
return pos_errors, aim_errors
|
||||
|
||||
|
||||
def find_optimal_parameters(frames: List[Frame]) -> dict:
|
||||
"""Search for optimal AdaptiveSensitivity (no scaling factor needed)."""
|
||||
print("\nSearching for optimal AdaptiveSensitivity...")
|
||||
print("-" * 60)
|
||||
|
||||
best_score = float('inf')
|
||||
best_sensitivity = 1.0
|
||||
|
||||
# Search grid — only sensitivity, no more scaling factor
|
||||
sensitivities = [0.1, 0.25, 0.5, 0.75, 1.0, 1.5, 2.0, 2.5, 3.0, 4.0, 5.0, 7.5, 10.0, 15.0, 20.0, 30.0, 50.0, 75.0, 100.0, 150.0, 200.0]
|
||||
|
||||
results = []
|
||||
|
||||
for sens in sensitivities:
|
||||
pos_errors, aim_errors = simulate_adaptive(frames, sens)
|
||||
if not pos_errors:
|
||||
continue
|
||||
pos_errors_s = sorted(pos_errors)
|
||||
aim_errors_s = sorted(aim_errors) if aim_errors else [0]
|
||||
|
||||
pos_mean = sum(pos_errors) / len(pos_errors)
|
||||
pos_p95 = pos_errors_s[int(len(pos_errors_s) * 0.95)]
|
||||
aim_mean = sum(aim_errors) / len(aim_errors) if aim_errors else 0
|
||||
aim_p95 = aim_errors_s[int(len(aim_errors_s) * 0.95)] if aim_errors else 0
|
||||
|
||||
# Score: combine position and aim errors (aim weighted more since it's the main issue)
|
||||
score = pos_mean * 0.3 + pos_p95 * 0.2 + aim_mean * 0.3 + aim_p95 * 0.2
|
||||
|
||||
results.append((sens, pos_mean, pos_p95, aim_mean, aim_p95, score))
|
||||
|
||||
if score < best_score:
|
||||
best_score = score
|
||||
best_sensitivity = sens
|
||||
|
||||
# Print results
|
||||
results.sort(key=lambda x: x[5])
|
||||
print(f"\n{'Sensitivity':>12} {'Pos Mean':>10} {'Pos P95':>10} {'Aim Mean':>10} {'Aim P95':>10} {'Score':>10}")
|
||||
print("-" * 65)
|
||||
for sens, pm, pp, am, ap, score in results[:10]:
|
||||
marker = " <-- BEST" if sens == best_sensitivity else ""
|
||||
print(f"{sens:>12.2f} {pm:>10.3f} {pp:>10.3f} {am:>10.3f} {ap:>10.3f} {score:>10.3f}{marker}")
|
||||
|
||||
return {
|
||||
'sensitivity': best_sensitivity,
|
||||
'score': best_score,
|
||||
}
|
||||
|
||||
|
||||
def main():
|
||||
if len(sys.argv) < 2:
|
||||
print("Usage: python analyze_antirecoil.py <csv_file> [--plot]")
|
||||
print("\nCSV files are saved by the EBBarrel component in:")
|
||||
print(" <Project>/Saved/Logs/AntiRecoil_*.csv")
|
||||
sys.exit(1)
|
||||
|
||||
csv_path = sys.argv[1]
|
||||
do_plot = '--plot' in sys.argv
|
||||
|
||||
if not os.path.exists(csv_path):
|
||||
print(f"Error: File not found: {csv_path}")
|
||||
sys.exit(1)
|
||||
|
||||
print(f"Loading: {csv_path}")
|
||||
frames = load_csv(csv_path)
|
||||
print(f"Loaded {len(frames)} frames")
|
||||
|
||||
if len(frames) < 50:
|
||||
print("Warning: very few frames. Record at least a few seconds of movement for good results.")
|
||||
|
||||
# Basic stats
|
||||
duration = frames[-1].timestamp - frames[0].timestamp
|
||||
avg_fps = len(frames) / duration if duration > 0 else 0
|
||||
print(f"Duration: {duration:.1f}s | Avg FPS: {avg_fps:.1f}")
|
||||
print(f"Avg safe samples: {sum(f.safe_count for f in frames) / len(frames):.1f}")
|
||||
print(f"Avg buffer samples: {sum(f.buffer_count for f in frames) / len(frames):.1f}")
|
||||
print(f"Avg extrapolation time: {sum(f.extrap_time for f in frames) / len(frames) * 1000:.1f}ms")
|
||||
|
||||
# Current prediction error (as recorded)
|
||||
print("\n=== CURRENT PREDICTION ERROR (as recorded) ===")
|
||||
current_err = compute_prediction_error(frames)
|
||||
print(f"Position error - Mean: {current_err['pos_mean']:.3f}cm | P95: {current_err['pos_p95']:.3f}cm | Max: {current_err['pos_max']:.3f}cm")
|
||||
print(f"Aim error - Mean: {current_err['aim_mean']:.3f}deg | P95: {current_err['aim_p95']:.3f}deg | Max: {current_err['aim_max']:.3f}deg")
|
||||
|
||||
# Find optimal parameters
|
||||
print("\n=== PARAMETER OPTIMIZATION ===")
|
||||
optimal = find_optimal_parameters(frames)
|
||||
|
||||
print(f"\n{'=' * 50}")
|
||||
print(f" RECOMMENDED SETTINGS:")
|
||||
print(f" AdaptiveSensitivity = {optimal['sensitivity']:.2f}")
|
||||
print(f"{'=' * 50}")
|
||||
|
||||
# Plotting
|
||||
if do_plot:
|
||||
try:
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
fig, axes = plt.subplots(3, 1, figsize=(14, 10), sharex=True)
|
||||
|
||||
timestamps = [f.timestamp - frames[0].timestamp for f in frames]
|
||||
|
||||
# Position error
|
||||
pos_errors = [vec_dist(f.pred_pos, f.real_pos) for f in frames]
|
||||
axes[0].plot(timestamps, pos_errors, 'r-', alpha=0.5, linewidth=0.5)
|
||||
axes[0].set_ylabel('Position Error (cm)')
|
||||
axes[0].set_title('Prediction Error Over Time')
|
||||
axes[0].axhline(y=current_err['pos_mean'], color='r', linestyle='--', alpha=0.5, label=f"Mean: {current_err['pos_mean']:.2f}cm")
|
||||
axes[0].legend()
|
||||
|
||||
# Aim error
|
||||
aim_errors = []
|
||||
for f in frames:
|
||||
a = vec_normalize(f.pred_aim)
|
||||
b = vec_normalize(f.real_aim)
|
||||
if vec_len(a) > 0.5 and vec_len(b) > 0.5:
|
||||
aim_errors.append(angle_between(a, b))
|
||||
else:
|
||||
aim_errors.append(0)
|
||||
axes[1].plot(timestamps, aim_errors, 'b-', alpha=0.5, linewidth=0.5)
|
||||
axes[1].set_ylabel('Aim Error (degrees)')
|
||||
axes[1].axhline(y=current_err['aim_mean'], color='b', linestyle='--', alpha=0.5, label=f"Mean: {current_err['aim_mean']:.2f}deg")
|
||||
axes[1].legend()
|
||||
|
||||
# Speed (from real positions)
|
||||
speeds = [0]
|
||||
for i in range(1, len(frames)):
|
||||
dt = frames[i].timestamp - frames[i-1].timestamp
|
||||
if dt > 1e-6:
|
||||
d = vec_dist(frames[i].real_pos, frames[i-1].real_pos)
|
||||
speeds.append(d / dt)
|
||||
else:
|
||||
speeds.append(speeds[-1])
|
||||
axes[2].plot(timestamps, speeds, 'g-', alpha=0.7, linewidth=0.5)
|
||||
axes[2].set_ylabel('Speed (cm/s)')
|
||||
axes[2].set_xlabel('Time (s)')
|
||||
|
||||
plt.tight_layout()
|
||||
plot_path = csv_path.replace('.csv', '_analysis.png')
|
||||
plt.savefig(plot_path, dpi=150)
|
||||
print(f"\nPlot saved: {plot_path}")
|
||||
plt.show()
|
||||
|
||||
except ImportError:
|
||||
print("\nmatplotlib not installed. Install with: pip install matplotlib")
|
||||
|
||||
print("\nDone.")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -45,6 +45,10 @@ void UEBBarrel::TriggerDebugIMUShock()
|
||||
|
||||
DebugIMUShockAimOffset = RandomDir * FMath::DegreesToRadians(DebugIMUShockAngle);
|
||||
DebugIMUShockPosOffset = RandomDir * DebugIMUShockPosition;
|
||||
|
||||
// Recoil timing: for simulated shocks, the corruption will appear in the buffer
|
||||
// on subsequent ticks. No need to capture here — the continuous analysis in
|
||||
// TickComponent will detect it automatically.
|
||||
}
|
||||
|
||||
void UEBBarrel::UpdateTransformHistory()
|
||||
@ -89,7 +93,11 @@ void UEBBarrel::UpdateTransformHistory()
|
||||
TransformHistory.Add(Sample);
|
||||
|
||||
// Trim buffer: remove samples older than AntiRecoilBufferTime
|
||||
double OldestAllowed = CurrentTime - FMath::Max(0.05f, AntiRecoilBufferTime);
|
||||
// During calibration, keep a larger buffer (0.5s min) for reliable 3-sigma analysis
|
||||
float EffectiveBufferTime = CalibrateAntiRecoil
|
||||
? FMath::Max(AntiRecoilBufferTime, 0.5f)
|
||||
: AntiRecoilBufferTime;
|
||||
double OldestAllowed = CurrentTime - FMath::Max(0.05f, EffectiveBufferTime);
|
||||
while (TransformHistory.Num() > 0 && TransformHistory[0].Timestamp < OldestAllowed)
|
||||
{
|
||||
TransformHistory.RemoveAt(0);
|
||||
@ -158,6 +166,26 @@ void UEBBarrel::ComputeAntiRecoilTransform()
|
||||
}
|
||||
break;
|
||||
|
||||
case EAntiRecoilMode::ARM_WeightedLinearRegression:
|
||||
{
|
||||
int32 SafeN = GetSafeCount(TransformHistory, GetWorld()->GetTimeSeconds(), AntiRecoilDiscardTime);
|
||||
if (SafeN >= 2)
|
||||
{
|
||||
PredictWeightedLinearRegression(GetWorld()->GetTimeSeconds(), Location, Aim);
|
||||
}
|
||||
else if (TransformHistory.Num() > 0)
|
||||
{
|
||||
Aim = TransformHistory[0].Aim;
|
||||
Location = TransformHistory[0].Location;
|
||||
}
|
||||
else
|
||||
{
|
||||
Aim = GetComponentTransform().GetUnitAxis(EAxis::X);
|
||||
Location = GetComponentTransform().GetLocation();
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case EAntiRecoilMode::ARM_KalmanFilter:
|
||||
{
|
||||
int32 SafeN = GetSafeCount(TransformHistory, GetWorld()->GetTimeSeconds(), AntiRecoilDiscardTime);
|
||||
@ -180,6 +208,26 @@ void UEBBarrel::ComputeAntiRecoilTransform()
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case EAntiRecoilMode::ARM_AdaptiveExtrapolation:
|
||||
{
|
||||
int32 SafeN = GetSafeCount(TransformHistory, GetWorld()->GetTimeSeconds(), AntiRecoilDiscardTime);
|
||||
if (SafeN >= 2)
|
||||
{
|
||||
PredictAdaptiveExtrapolation(GetWorld()->GetTimeSeconds(), Location, Aim);
|
||||
}
|
||||
else if (TransformHistory.Num() > 0)
|
||||
{
|
||||
Aim = TransformHistory[0].Aim;
|
||||
Location = TransformHistory[0].Location;
|
||||
}
|
||||
else
|
||||
{
|
||||
Aim = GetComponentTransform().GetUnitAxis(EAxis::X);
|
||||
Location = GetComponentTransform().GetLocation();
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
@ -229,7 +277,14 @@ void UEBBarrel::PredictLinearExtrapolation(double CurrentTime, FVector& OutLocat
|
||||
const FTimestampedTransform& LastSafe = TransformHistory[SafeN - 1];
|
||||
double ExtrapolationTime = CurrentTime - LastSafe.Timestamp;
|
||||
|
||||
OutLocation = LastSafe.Location + AvgLinearVelocity * ExtrapolationTime;
|
||||
// Apply optional velocity damping: exponential decay toward zero
|
||||
float DampingScale = 1.0f;
|
||||
if (ExtrapolationDamping > 0.0f)
|
||||
{
|
||||
DampingScale = FMath::Exp(-ExtrapolationDamping * (float)ExtrapolationTime);
|
||||
}
|
||||
|
||||
OutLocation = LastSafe.Location + AvgLinearVelocity * ExtrapolationTime * DampingScale;
|
||||
|
||||
// Angular extrapolation using quaternion slerp
|
||||
// Use first and last safe samples for rotation direction
|
||||
@ -241,7 +296,7 @@ void UEBBarrel::PredictLinearExtrapolation(double CurrentTime, FVector& OutLocat
|
||||
FQuat FirstQuat = FRotationMatrix::MakeFromX(FirstSafe.Aim).ToQuat();
|
||||
FQuat LastQuat = FRotationMatrix::MakeFromX(LastSafe.Aim).ToQuat();
|
||||
|
||||
double TotalAlpha = ExtrapolationTime / SafeDeltaT;
|
||||
double TotalAlpha = ExtrapolationTime / SafeDeltaT * DampingScale;
|
||||
FQuat PredictedQuat = FQuat::Slerp(FirstQuat, LastQuat, 1.0 + TotalAlpha);
|
||||
|
||||
OutAim = PredictedQuat.GetForwardVector().GetSafeNormal();
|
||||
@ -257,10 +312,10 @@ void UEBBarrel::PredictLinearExtrapolation(double CurrentTime, FVector& OutLocat
|
||||
}
|
||||
|
||||
// --- Weighted Linear Regression ---
|
||||
// Fits a weighted least-squares line through only the SAFE samples, extrapolates to current time.
|
||||
// More recent safe samples get higher weight.
|
||||
// Fits a weighted least-squares line (y = a + bt) through SAFE samples, extrapolates to current time.
|
||||
// Simple and stable. More recent safe samples get higher weight.
|
||||
|
||||
void UEBBarrel::PredictWeightedRegression(double CurrentTime, FVector& OutLocation, FVector& OutAim) const
|
||||
void UEBBarrel::PredictWeightedLinearRegression(double CurrentTime, FVector& OutLocation, FVector& OutAim) const
|
||||
{
|
||||
const int32 SafeN = GetSafeCount(TransformHistory, GetWorld()->GetTimeSeconds(), AntiRecoilDiscardTime);
|
||||
if (SafeN < 2)
|
||||
@ -270,11 +325,8 @@ void UEBBarrel::PredictWeightedRegression(double CurrentTime, FVector& OutLocati
|
||||
return;
|
||||
}
|
||||
|
||||
// Use timestamps relative to the first safe sample to avoid precision issues
|
||||
double T0 = TransformHistory[0].Timestamp;
|
||||
|
||||
// Weighted linear regression: y = a + b * t
|
||||
// Weights: linearly increasing (1, 2, 3, ..., SafeN)
|
||||
double SumW = 0.0;
|
||||
double SumWT = 0.0;
|
||||
double SumWTT = 0.0;
|
||||
@ -285,7 +337,7 @@ void UEBBarrel::PredictWeightedRegression(double CurrentTime, FVector& OutLocati
|
||||
|
||||
for (int32 i = 0; i < SafeN; i++)
|
||||
{
|
||||
double w = FMath::Pow((double)(i + 1), (double)RegressionWeightExponent); // Weight curve controlled by exponent
|
||||
double w = FMath::Pow((double)(i + 1), (double)RegressionWeightExponent);
|
||||
double t = TransformHistory[i].Timestamp - T0;
|
||||
|
||||
SumW += w;
|
||||
@ -305,19 +357,25 @@ void UEBBarrel::PredictWeightedRegression(double CurrentTime, FVector& OutLocati
|
||||
return;
|
||||
}
|
||||
|
||||
// Solve for position: intercept (a) and slope (b)
|
||||
FVector PosIntercept = (SumWY * SumWTT - SumWTY * SumWT) / Det;
|
||||
FVector PosSlope = (SumWTY * SumW - SumWY * SumWT) / Det;
|
||||
|
||||
// Solve for aim: intercept (a) and slope (b)
|
||||
FVector AimIntercept = (SumWAim * SumWTT - SumWTAim * SumWT) / Det;
|
||||
FVector AimSlope = (SumWTAim * SumW - SumWAim * SumWT) / Det;
|
||||
|
||||
// Extrapolate to current time
|
||||
double TPred = CurrentTime - T0;
|
||||
OutLocation = PosIntercept + PosSlope * TPred;
|
||||
|
||||
FVector PredAim = AimIntercept + AimSlope * TPred;
|
||||
// Apply optional velocity damping
|
||||
float DampingScale = 1.0f;
|
||||
if (ExtrapolationDamping > 0.0f)
|
||||
{
|
||||
double TLastSafe = TransformHistory[SafeN - 1].Timestamp - T0;
|
||||
float ExtrapolationTime = (float)(TPred - TLastSafe);
|
||||
DampingScale = FMath::Exp(-ExtrapolationDamping * ExtrapolationTime);
|
||||
}
|
||||
|
||||
OutLocation = PosIntercept + PosSlope * TPred * DampingScale;
|
||||
|
||||
FVector PredAim = AimIntercept + AimSlope * TPred * DampingScale;
|
||||
OutAim = PredAim.GetSafeNormal();
|
||||
if (OutAim.IsNearlyZero())
|
||||
{
|
||||
@ -325,6 +383,226 @@ void UEBBarrel::PredictWeightedRegression(double CurrentTime, FVector& OutLocati
|
||||
}
|
||||
}
|
||||
|
||||
// --- Weighted Quadratic Regression ---
|
||||
// Fits a weighted least-squares quadratic (y = a + bt + ct^2) through SAFE samples.
|
||||
// Captures deceleration naturally: if user stops before firing, c < 0 curves prediction toward stop.
|
||||
// Falls back to linear fit if < 3 samples or ill-conditioned 3x3 system.
|
||||
|
||||
void UEBBarrel::PredictWeightedRegression(double CurrentTime, FVector& OutLocation, FVector& OutAim) const
|
||||
{
|
||||
const int32 SafeN = GetSafeCount(TransformHistory, GetWorld()->GetTimeSeconds(), AntiRecoilDiscardTime);
|
||||
if (SafeN < 2)
|
||||
{
|
||||
OutLocation = TransformHistory[0].Location;
|
||||
OutAim = TransformHistory[0].Aim;
|
||||
return;
|
||||
}
|
||||
|
||||
// Use timestamps relative to the first safe sample to avoid precision issues
|
||||
double T0 = TransformHistory[0].Timestamp;
|
||||
|
||||
// Accumulate weighted sums for quadratic regression: y = a + b*t + c*t^2
|
||||
double SumW = 0.0;
|
||||
double SumWT = 0.0;
|
||||
double SumWTT = 0.0;
|
||||
double SumWTTT = 0.0;
|
||||
double SumWTTTT = 0.0;
|
||||
FVector SumWY = FVector::ZeroVector;
|
||||
FVector SumWTY = FVector::ZeroVector;
|
||||
FVector SumWTTY = FVector::ZeroVector;
|
||||
FVector SumWAim = FVector::ZeroVector;
|
||||
FVector SumWTAim = FVector::ZeroVector;
|
||||
FVector SumWTTAim = FVector::ZeroVector;
|
||||
|
||||
for (int32 i = 0; i < SafeN; i++)
|
||||
{
|
||||
double w = FMath::Pow((double)(i + 1), (double)RegressionWeightExponent);
|
||||
double t = TransformHistory[i].Timestamp - T0;
|
||||
double tt = t * t;
|
||||
|
||||
SumW += w;
|
||||
SumWT += w * t;
|
||||
SumWTT += w * tt;
|
||||
SumWTTT += w * tt * t;
|
||||
SumWTTTT += w * tt * tt;
|
||||
SumWY += TransformHistory[i].Location * w;
|
||||
SumWTY += TransformHistory[i].Location * (w * t);
|
||||
SumWTTY += TransformHistory[i].Location * (w * tt);
|
||||
SumWAim += TransformHistory[i].Aim * w;
|
||||
SumWTAim += TransformHistory[i].Aim * (w * t);
|
||||
SumWTTAim += TransformHistory[i].Aim * (w * tt);
|
||||
}
|
||||
|
||||
double TPred = CurrentTime - T0;
|
||||
double TLastSafe = TransformHistory[SafeN - 1].Timestamp - T0;
|
||||
|
||||
// Try quadratic fit (3x3 system) if we have enough samples
|
||||
bool bUseQuadratic = false;
|
||||
FVector PosA, PosB, PosC;
|
||||
FVector AimA, AimB, AimC;
|
||||
|
||||
if (SafeN >= 3)
|
||||
{
|
||||
// Normal equations for weighted quadratic: M * [a,b,c]^T = R
|
||||
// M = | SumW SumWT SumWTT |
|
||||
// | SumWT SumWTT SumWTTT |
|
||||
// | SumWTT SumWTTT SumWTTTT |
|
||||
// Solve by Cramer's rule (3x3 determinant)
|
||||
double M00 = SumW, M01 = SumWT, M02 = SumWTT;
|
||||
double M10 = SumWT, M11 = SumWTT, M12 = SumWTTT;
|
||||
double M20 = SumWTT, M21 = SumWTTT, M22 = SumWTTTT;
|
||||
|
||||
double Det3 = M00 * (M11 * M22 - M12 * M21)
|
||||
- M01 * (M10 * M22 - M12 * M20)
|
||||
+ M02 * (M10 * M21 - M11 * M20);
|
||||
|
||||
if (FMath::Abs(Det3) > SMALL_NUMBER)
|
||||
{
|
||||
bUseQuadratic = true;
|
||||
double InvDet = 1.0 / Det3;
|
||||
|
||||
// Cofactors for Cramer's rule
|
||||
double C00 = M11 * M22 - M12 * M21;
|
||||
double C01 = -(M10 * M22 - M12 * M20);
|
||||
double C02 = M10 * M21 - M11 * M20;
|
||||
double C10 = -(M01 * M22 - M02 * M21);
|
||||
double C11 = M00 * M22 - M02 * M20;
|
||||
double C12 = -(M00 * M21 - M01 * M20);
|
||||
double C20 = M01 * M12 - M02 * M11;
|
||||
double C21 = -(M00 * M12 - M02 * M10);
|
||||
double C22 = M00 * M11 - M01 * M10;
|
||||
|
||||
// Solve for position coefficients: a, b, c
|
||||
PosA = (SumWY * C00 + SumWTY * C10 + SumWTTY * C20) * InvDet;
|
||||
PosB = (SumWY * C01 + SumWTY * C11 + SumWTTY * C21) * InvDet;
|
||||
PosC = (SumWY * C02 + SumWTY * C12 + SumWTTY * C22) * InvDet;
|
||||
|
||||
// Solve for aim coefficients: a, b, c
|
||||
AimA = (SumWAim * C00 + SumWTAim * C10 + SumWTTAim * C20) * InvDet;
|
||||
AimB = (SumWAim * C01 + SumWTAim * C11 + SumWTTAim * C21) * InvDet;
|
||||
AimC = (SumWAim * C02 + SumWTAim * C12 + SumWTTAim * C22) * InvDet;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// Always compute linear result (2x2 system)
|
||||
FVector LinearLocation, LinearAim;
|
||||
{
|
||||
double Det = SumW * SumWTT - SumWT * SumWT;
|
||||
if (FMath::Abs(Det) <= SMALL_NUMBER)
|
||||
{
|
||||
OutLocation = TransformHistory[SafeN - 1].Location;
|
||||
OutAim = TransformHistory[SafeN - 1].Aim;
|
||||
return;
|
||||
}
|
||||
|
||||
FVector PosIntercept = (SumWY * SumWTT - SumWTY * SumWT) / Det;
|
||||
FVector PosSlope = (SumWTY * SumW - SumWY * SumWT) / Det;
|
||||
FVector AimIntercept = (SumWAim * SumWTT - SumWTAim * SumWT) / Det;
|
||||
FVector AimSlope = (SumWTAim * SumW - SumWAim * SumWT) / Det;
|
||||
|
||||
LinearLocation = PosIntercept + PosSlope * TPred;
|
||||
FVector PredAim = AimIntercept + AimSlope * TPred;
|
||||
LinearAim = PredAim.GetSafeNormal();
|
||||
if (LinearAim.IsNearlyZero())
|
||||
{
|
||||
LinearAim = TransformHistory[SafeN - 1].Aim;
|
||||
}
|
||||
}
|
||||
|
||||
if (bUseQuadratic)
|
||||
{
|
||||
// Compute quadratic result with velocity-reversal clamping
|
||||
FVector QuadLocation;
|
||||
for (int32 Axis = 0; Axis < 3; Axis++)
|
||||
{
|
||||
double b = PosB[Axis];
|
||||
double c = PosC[Axis];
|
||||
double VelAtLastSafe = b + 2.0 * c * TLastSafe;
|
||||
double VelAtPred = b + 2.0 * c * TPred;
|
||||
double TUse = TPred;
|
||||
|
||||
if (VelAtLastSafe * VelAtPred < 0.0 && FMath::Abs(c) > SMALL_NUMBER)
|
||||
{
|
||||
double TStop = -b / (2.0 * c);
|
||||
if (TStop > TLastSafe && TStop < TPred)
|
||||
{
|
||||
TUse = TStop;
|
||||
}
|
||||
}
|
||||
|
||||
QuadLocation[Axis] = PosA[Axis] + b * TUse + c * TUse * TUse;
|
||||
}
|
||||
|
||||
FVector QuadAimVec;
|
||||
for (int32 Axis = 0; Axis < 3; Axis++)
|
||||
{
|
||||
double b = AimB[Axis];
|
||||
double c = AimC[Axis];
|
||||
double VelAtLastSafe = b + 2.0 * c * TLastSafe;
|
||||
double VelAtPred = b + 2.0 * c * TPred;
|
||||
double TUse = TPred;
|
||||
|
||||
if (VelAtLastSafe * VelAtPred < 0.0 && FMath::Abs(c) > SMALL_NUMBER)
|
||||
{
|
||||
double TStop = -b / (2.0 * c);
|
||||
if (TStop > TLastSafe && TStop < TPred)
|
||||
{
|
||||
TUse = TStop;
|
||||
}
|
||||
}
|
||||
|
||||
QuadAimVec[Axis] = AimA[Axis] + b * TUse + c * TUse * TUse;
|
||||
}
|
||||
|
||||
FVector QuadAim = QuadAimVec.GetSafeNormal();
|
||||
if (QuadAim.IsNearlyZero())
|
||||
{
|
||||
QuadAim = TransformHistory[SafeN - 1].Aim;
|
||||
}
|
||||
|
||||
// Smooth blend between linear and quadratic based on significance of c.
|
||||
// Alpha = 0 → pure linear, Alpha = 1 → pure quadratic.
|
||||
// Ramp from 0 to 1 as max(|c*t^2| / |b*t|) goes from 0.05 to 0.20.
|
||||
double PosCRatio = (FMath::Max(PosB.GetAbsMax() * TPred, SMALL_NUMBER) > SMALL_NUMBER)
|
||||
? (PosC.GetAbsMax() * TPred * TPred) / (PosB.GetAbsMax() * TPred) : 0.0;
|
||||
double AimCRatio = (FMath::Max(AimB.GetAbsMax() * TPred, SMALL_NUMBER) > SMALL_NUMBER)
|
||||
? (AimC.GetAbsMax() * TPred * TPred) / (AimB.GetAbsMax() * TPred) : 0.0;
|
||||
double MaxRatio = FMath::Max(PosCRatio, AimCRatio);
|
||||
float BlendAlpha = (float)FMath::Clamp((MaxRatio - 0.05) / (0.20 - 0.05), 0.0, 1.0);
|
||||
|
||||
OutLocation = FMath::Lerp(LinearLocation, QuadLocation, BlendAlpha);
|
||||
OutAim = FMath::Lerp(LinearAim, QuadAim, BlendAlpha).GetSafeNormal();
|
||||
if (OutAim.IsNearlyZero())
|
||||
{
|
||||
OutAim = TransformHistory[SafeN - 1].Aim;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
OutLocation = LinearLocation;
|
||||
OutAim = LinearAim;
|
||||
}
|
||||
|
||||
// Apply optional velocity damping on the final result
|
||||
// Blend toward the last safe sample position (i.e., reduce the extrapolation offset)
|
||||
if (ExtrapolationDamping > 0.0f)
|
||||
{
|
||||
float ExtrapolationTime = (float)(TPred - TLastSafe);
|
||||
float DampingScale = FMath::Exp(-ExtrapolationDamping * ExtrapolationTime);
|
||||
FVector LastSafeLocation = TransformHistory[SafeN - 1].Location;
|
||||
FVector LastSafeAim = TransformHistory[SafeN - 1].Aim;
|
||||
|
||||
// Damping blends from full extrapolation (DampingScale=1) toward last safe sample (DampingScale=0)
|
||||
OutLocation = LastSafeLocation + (OutLocation - LastSafeLocation) * DampingScale;
|
||||
OutAim = FMath::Lerp(LastSafeAim, OutAim, DampingScale).GetSafeNormal();
|
||||
if (OutAim.IsNearlyZero())
|
||||
{
|
||||
OutAim = LastSafeAim;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// --- Simplified Kalman Filter ---
|
||||
// Maintains state estimate [position, velocity, aim, angular_velocity]
|
||||
// Only fed with SAFE (non-contaminated) measurements, predicts forward to current time
|
||||
@ -403,12 +681,155 @@ void UEBBarrel::PredictKalmanFilter(double CurrentTime, FVector& OutLocation, FV
|
||||
// Extrapolate from Kalman state to current time
|
||||
float dt = (float)(CurrentTime - KalmanLastTimestamp);
|
||||
|
||||
OutLocation = KalmanPosition + KalmanVelocity * dt;
|
||||
// Apply optional velocity damping
|
||||
float DampingScale = 1.0f;
|
||||
if (ExtrapolationDamping > 0.0f)
|
||||
{
|
||||
DampingScale = FMath::Exp(-ExtrapolationDamping * dt);
|
||||
}
|
||||
|
||||
FVector PredAim = KalmanAim + KalmanAngularVelocity * dt;
|
||||
OutLocation = KalmanPosition + KalmanVelocity * dt * DampingScale;
|
||||
|
||||
FVector PredAim = KalmanAim + KalmanAngularVelocity * dt * DampingScale;
|
||||
OutAim = PredAim.GetSafeNormal();
|
||||
if (OutAim.IsNearlyZero())
|
||||
{
|
||||
OutAim = KalmanAim.GetSafeNormal();
|
||||
}
|
||||
}
|
||||
|
||||
// --- Adaptive Extrapolation ---
|
||||
// Computes weighted average velocity from safe samples, then scales extrapolation
|
||||
// by a confidence factor based on velocity consistency.
|
||||
// Low velocity variance → full extrapolation (steady movement).
|
||||
// High velocity variance → reduced extrapolation (deceleration/direction change).
|
||||
|
||||
void UEBBarrel::PredictAdaptiveExtrapolation(double CurrentTime, FVector& OutLocation, FVector& OutAim) const
|
||||
{
|
||||
const int32 SafeN = GetSafeCount(TransformHistory, GetWorld()->GetTimeSeconds(), AntiRecoilDiscardTime);
|
||||
if (SafeN < 2)
|
||||
{
|
||||
OutLocation = TransformHistory[0].Location;
|
||||
OutAim = TransformHistory[0].Aim;
|
||||
return;
|
||||
}
|
||||
|
||||
// Compute weighted velocities from consecutive safe sample pairs
|
||||
// Weight recent pairs more heavily
|
||||
TArray<FVector> PosVelocities;
|
||||
TArray<FVector> AimVelocities;
|
||||
TArray<double> Weights;
|
||||
PosVelocities.Reserve(SafeN - 1);
|
||||
AimVelocities.Reserve(SafeN - 1);
|
||||
Weights.Reserve(SafeN - 1);
|
||||
|
||||
for (int32 i = 1; i < SafeN; i++)
|
||||
{
|
||||
double dt = TransformHistory[i].Timestamp - TransformHistory[i - 1].Timestamp;
|
||||
if (dt > SMALL_NUMBER)
|
||||
{
|
||||
FVector PosVel = (TransformHistory[i].Location - TransformHistory[i - 1].Location) / dt;
|
||||
FVector AimVel = (TransformHistory[i].Aim - TransformHistory[i - 1].Aim) / dt;
|
||||
// Weight: recent pairs get exponentially more weight
|
||||
double w = FMath::Pow((double)i, 2.0);
|
||||
PosVelocities.Add(PosVel);
|
||||
AimVelocities.Add(AimVel);
|
||||
Weights.Add(w);
|
||||
}
|
||||
}
|
||||
|
||||
if (PosVelocities.Num() == 0)
|
||||
{
|
||||
OutLocation = TransformHistory[SafeN - 1].Location;
|
||||
OutAim = TransformHistory[SafeN - 1].Aim;
|
||||
return;
|
||||
}
|
||||
|
||||
// Compute weighted average velocity
|
||||
double TotalWeight = 0.0;
|
||||
FVector AvgPosVel = FVector::ZeroVector;
|
||||
FVector AvgAimVel = FVector::ZeroVector;
|
||||
|
||||
for (int32 i = 0; i < PosVelocities.Num(); i++)
|
||||
{
|
||||
AvgPosVel += PosVelocities[i] * Weights[i];
|
||||
AvgAimVel += AimVelocities[i] * Weights[i];
|
||||
TotalWeight += Weights[i];
|
||||
}
|
||||
|
||||
AvgPosVel /= TotalWeight;
|
||||
AvgAimVel /= TotalWeight;
|
||||
|
||||
// Compute weighted variance of velocity (how consistent is the velocity?)
|
||||
FVector PosVelVariance = FVector::ZeroVector;
|
||||
FVector AimVelVariance = FVector::ZeroVector;
|
||||
|
||||
for (int32 i = 0; i < PosVelocities.Num(); i++)
|
||||
{
|
||||
FVector PosDiff = PosVelocities[i] - AvgPosVel;
|
||||
FVector AimDiff = AimVelocities[i] - AvgAimVel;
|
||||
PosVelVariance += FVector(PosDiff.X * PosDiff.X, PosDiff.Y * PosDiff.Y, PosDiff.Z * PosDiff.Z) * Weights[i];
|
||||
AimVelVariance += FVector(AimDiff.X * AimDiff.X, AimDiff.Y * AimDiff.Y, AimDiff.Z * AimDiff.Z) * Weights[i];
|
||||
}
|
||||
|
||||
PosVelVariance /= TotalWeight;
|
||||
AimVelVariance /= TotalWeight;
|
||||
|
||||
// Compute separate confidence for position and aim.
|
||||
// Position variance is in cm^2/s^2 (large values), aim variance is in unit^2/s^2 (small values).
|
||||
// Using relative variance (normalized by speed^2) so both are scale-independent.
|
||||
double PosSpeed2 = AvgPosVel.SizeSquared();
|
||||
double AimSpeed2 = AvgAimVel.SizeSquared();
|
||||
|
||||
double PosAbsVariance = PosVelVariance.X + PosVelVariance.Y + PosVelVariance.Z;
|
||||
double AimAbsVariance = AimVelVariance.X + AimVelVariance.Y + AimVelVariance.Z;
|
||||
|
||||
// Extrapolate from last safe sample
|
||||
const FTimestampedTransform& LastSafe = TransformHistory[SafeN - 1];
|
||||
double ExtrapolationTime = CurrentTime - LastSafe.Timestamp;
|
||||
|
||||
// Relative variance = variance / speed^2. When speed is ~0, extrapolation offset
|
||||
// is negligible anyway so confidence doesn't matter — set to 1.
|
||||
// Low-speed protection: at low speed the extrapolation offset (speed * dt) is tiny,
|
||||
// so there's no overshoot to prevent. We blend confidence back toward 1.0 when the
|
||||
// potential extrapolation offset is small (< 1cm for position, < 0.01 for aim direction).
|
||||
float PosConfidence = 1.0f;
|
||||
if (PosSpeed2 > SMALL_NUMBER)
|
||||
{
|
||||
double PosRelVar = PosAbsVariance / PosSpeed2;
|
||||
PosConfidence = 1.0f / (1.0f + (float)PosRelVar * AdaptiveSensitivity);
|
||||
|
||||
// Low-speed protection: if extrapolation offset < 1cm, blend confidence toward 1
|
||||
float PosExtrapOffset = FMath::Sqrt((float)PosSpeed2) * (float)ExtrapolationTime;
|
||||
float PosOffsetSignificance = FMath::Clamp(PosExtrapOffset / 1.0f, 0.0f, 1.0f); // 1cm threshold
|
||||
PosConfidence = FMath::Lerp(1.0f, PosConfidence, PosOffsetSignificance);
|
||||
}
|
||||
|
||||
float AimConfidence = 1.0f;
|
||||
if (AimSpeed2 > SMALL_NUMBER)
|
||||
{
|
||||
double AimRelVar = AimAbsVariance / AimSpeed2;
|
||||
AimConfidence = 1.0f / (1.0f + (float)AimRelVar * AdaptiveSensitivity);
|
||||
|
||||
// Low-speed protection: if aim extrapolation offset is tiny, blend toward 1
|
||||
float AimExtrapOffset = FMath::Sqrt((float)AimSpeed2) * (float)ExtrapolationTime;
|
||||
float AimOffsetSignificance = FMath::Clamp(AimExtrapOffset / 0.01f, 0.0f, 1.0f); // 0.01 unit threshold
|
||||
AimConfidence = FMath::Lerp(1.0f, AimConfidence, AimOffsetSignificance);
|
||||
}
|
||||
|
||||
// Apply optional damping on top of confidence
|
||||
float DampingScale = 1.0f;
|
||||
if (ExtrapolationDamping > 0.0f)
|
||||
{
|
||||
DampingScale = FMath::Exp(-ExtrapolationDamping * (float)ExtrapolationTime);
|
||||
}
|
||||
|
||||
OutLocation = LastSafe.Location + AvgPosVel * ExtrapolationTime * (PosConfidence * DampingScale);
|
||||
|
||||
FVector PredAim = LastSafe.Aim + AvgAimVel * ExtrapolationTime * (AimConfidence * DampingScale);
|
||||
OutAim = PredAim.GetSafeNormal();
|
||||
if (OutAim.IsNearlyZero())
|
||||
{
|
||||
OutAim = LastSafe.Aim;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,9 +1,14 @@
|
||||
// Copyright 2018 Mookie. All Rights Reserved.
|
||||
#include "EBBarrel.h"
|
||||
#include "DrawDebugHelpers.h"
|
||||
#include "Engine/Engine.h"
|
||||
#include "HAL/PlatformFileManager.h"
|
||||
#include "Misc/Paths.h"
|
||||
#include "Misc/DateTime.h"
|
||||
|
||||
UEBBarrel::UEBBarrel() {
|
||||
PrimaryComponentTick.bCanEverTick = true;
|
||||
PrimaryComponentTick.TickGroup = TG_PostPhysics;
|
||||
bHiddenInGame = true;
|
||||
bAutoActivate = true;
|
||||
SetIsReplicatedByDefault(ReplicateVariables);
|
||||
@ -13,6 +18,20 @@ UEBBarrel::UEBBarrel() {
|
||||
GatlingRPS = FireRateMin;
|
||||
}
|
||||
|
||||
void UEBBarrel::BeginPlay()
|
||||
{
|
||||
Super::BeginPlay();
|
||||
|
||||
// Add tick prerequisite on the attach parent so this barrel ticks after
|
||||
// its parent's transform is updated. UE propagates transforms down the
|
||||
// attach chain, so this is sufficient even with deep hierarchies
|
||||
// (Pawn -> MotionController -> ChildActor -> Weapon -> EBBarrel).
|
||||
if (USceneComponent* Parent = GetAttachParent())
|
||||
{
|
||||
PrimaryComponentTick.AddPrerequisite(Parent, Parent->PrimaryComponentTick);
|
||||
}
|
||||
}
|
||||
|
||||
void UEBBarrel::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction)
|
||||
{
|
||||
Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||
@ -95,17 +114,170 @@ void UEBBarrel::TickComponent(float DeltaTime, ELevelTick TickType, FActorCompon
|
||||
{
|
||||
if (GetWorld()->GetTimeSeconds() < DebugIMUShockLineEndTime)
|
||||
{
|
||||
// Yellow line: uncorrected aim (where the bullet would have gone without anti-recoil)
|
||||
// Green persistent line: raw tracker aim at moment of shot (same color as real-time green)
|
||||
DrawDebugLine(GetWorld(), DebugIMUShockCapturedLocation,
|
||||
DebugIMUShockCapturedLocation + DebugIMUShockCapturedAim * DebugAntiRecoilLineLength,
|
||||
FColor::Yellow, false, -1.0f, 0, DebugAntiRecoilLineThickness);
|
||||
DrawDebugSphere(GetWorld(), DebugIMUShockCapturedLocation, 3.0f, 8, FColor::Yellow, false, -1.0f, 0, DebugAntiRecoilLineThickness);
|
||||
FColor::Green, false, -1.0f, 0, DebugAntiRecoilLineThickness);
|
||||
DrawDebugSphere(GetWorld(), DebugIMUShockCapturedLocation, 3.0f, 8, FColor::Green, false, -1.0f, 0, DebugAntiRecoilLineThickness);
|
||||
}
|
||||
else
|
||||
{
|
||||
DebugIMUShockLineCaptured = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Red persistent line: corrected aim retained for the shot (same color as real-time red)
|
||||
if (DebugCorrectedShotLineCaptured)
|
||||
{
|
||||
if (GetWorld()->GetTimeSeconds() < DebugCorrectedShotLineEndTime)
|
||||
{
|
||||
DrawDebugLine(GetWorld(), DebugCorrectedShotCapturedLocation,
|
||||
DebugCorrectedShotCapturedLocation + DebugCorrectedShotCapturedAim * DebugAntiRecoilLineLength,
|
||||
FColor::Red, false, -1.0f, 0, DebugAntiRecoilLineThickness);
|
||||
DrawDebugSphere(GetWorld(), DebugCorrectedShotCapturedLocation, 3.0f, 8, FColor::Red, false, -1.0f, 0, DebugAntiRecoilLineThickness);
|
||||
}
|
||||
else
|
||||
{
|
||||
DebugCorrectedShotLineCaptured = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// CSV Prediction Recording
|
||||
if (RecordPredictionCSV && AntiRecoilMode != EAntiRecoilMode::ARM_None)
|
||||
{
|
||||
if (!bCSVFileOpen)
|
||||
{
|
||||
// Open new CSV file
|
||||
FString Timestamp = FDateTime::Now().ToString(TEXT("%Y%m%d_%H%M%S"));
|
||||
CSVFilePath = FPaths::ProjectSavedDir() / TEXT("Logs") / FString::Printf(TEXT("AntiRecoil_%s.csv"), *Timestamp);
|
||||
CSVFileHandle = FPlatformFileManager::Get().GetPlatformFile().OpenWrite(*CSVFilePath);
|
||||
if (CSVFileHandle)
|
||||
{
|
||||
bCSVFileOpen = true;
|
||||
FString Header = TEXT("Timestamp,RealPosX,RealPosY,RealPosZ,RealAimX,RealAimY,RealAimZ,PredPosX,PredPosY,PredPosZ,PredAimX,PredAimY,PredAimZ,SafeCount,BufferCount,ExtrapolationTime\n");
|
||||
auto HeaderUtf8 = StringCast<ANSICHAR>(*Header);
|
||||
CSVFileHandle->Write((const uint8*)HeaderUtf8.Get(), HeaderUtf8.Length());
|
||||
|
||||
if (GEngine)
|
||||
{
|
||||
GEngine->AddOnScreenDebugMessage(-700, 5.0f, FColor::Cyan,
|
||||
FString::Printf(TEXT("CSV Recording started: %s"), *CSVFilePath));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (bCSVFileOpen && CSVFileHandle)
|
||||
{
|
||||
FVector RealPos = GetComponentTransform().GetLocation();
|
||||
FVector RealAim = GetComponentTransform().GetUnitAxis(EAxis::X);
|
||||
// Count safe samples (same logic as GetSafeCount in AntiRecoilPredict.cpp)
|
||||
double SafeCutoff = GetWorld()->GetTimeSeconds() - AntiRecoilDiscardTime;
|
||||
int32 SafeN = 0;
|
||||
for (int32 si = 0; si < TransformHistory.Num(); si++)
|
||||
{
|
||||
if (TransformHistory[si].Timestamp < SafeCutoff) SafeN++;
|
||||
}
|
||||
double ExtrapTime = (SafeN > 0) ? (GetWorld()->GetTimeSeconds() - TransformHistory[SafeN - 1].Timestamp) : 0.0;
|
||||
|
||||
FString Line = FString::Printf(TEXT("%.6f,%.4f,%.4f,%.4f,%.6f,%.6f,%.6f,%.4f,%.4f,%.4f,%.6f,%.6f,%.6f,%d,%d,%.6f\n"),
|
||||
GetWorld()->GetTimeSeconds(),
|
||||
RealPos.X, RealPos.Y, RealPos.Z,
|
||||
RealAim.X, RealAim.Y, RealAim.Z,
|
||||
Location.X, Location.Y, Location.Z,
|
||||
Aim.X, Aim.Y, Aim.Z,
|
||||
SafeN, TransformHistory.Num(), ExtrapTime);
|
||||
auto LineUtf8 = StringCast<ANSICHAR>(*Line);
|
||||
CSVFileHandle->Write((const uint8*)LineUtf8.Get(), LineUtf8.Length());
|
||||
}
|
||||
}
|
||||
else if (bCSVFileOpen)
|
||||
{
|
||||
// Close CSV file
|
||||
if (CSVFileHandle)
|
||||
{
|
||||
delete CSVFileHandle;
|
||||
CSVFileHandle = nullptr;
|
||||
}
|
||||
bCSVFileOpen = false;
|
||||
|
||||
if (GEngine)
|
||||
{
|
||||
GEngine->AddOnScreenDebugMessage(-700, 5.0f, FColor::Cyan,
|
||||
FString::Printf(TEXT("CSV Recording stopped: %s"), *CSVFilePath));
|
||||
}
|
||||
}
|
||||
|
||||
// Calibration HUD
|
||||
if (CalibrateAntiRecoil && GEngine)
|
||||
{
|
||||
int32 CalKey = -600;
|
||||
FColor CalTitle = FColor::Magenta;
|
||||
FColor CalValue = FColor::White;
|
||||
FColor CalGood = FColor::Green;
|
||||
FColor CalWarn = FColor::Yellow;
|
||||
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalTitle,
|
||||
TEXT("====== ANTI-RECOIL CALIBRATION ======"));
|
||||
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalValue,
|
||||
FString::Printf(TEXT(" Collecting: %d / %d shots"),
|
||||
CalibrationShotsCollected, CalibrationShotCount));
|
||||
|
||||
// Running stats from current sequence
|
||||
if (CalibrationShots.Num() > 0)
|
||||
{
|
||||
// Quick re-analysis of already captured shots for running display
|
||||
float RunMax = 0.0f, RunSum = 0.0f;
|
||||
int32 RunCount = 0;
|
||||
for (const FCalibrationShotData& S : CalibrationShots)
|
||||
{
|
||||
// Simplified: use buffer size as rough proxy until full analysis
|
||||
// We'll show "pending analysis" for individual shots
|
||||
RunCount++;
|
||||
}
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalValue,
|
||||
FString::Printf(TEXT(" %d shots captured, awaiting sequence completion..."), RunCount));
|
||||
}
|
||||
else
|
||||
{
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalValue,
|
||||
TEXT(" Fire weapon to collect data..."));
|
||||
}
|
||||
|
||||
// Show last completed sequence results
|
||||
if (LastCalibrationResult.bValid)
|
||||
{
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalTitle,
|
||||
TEXT(" --- Last Sequence Results ---"));
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalGood,
|
||||
FString::Printf(TEXT(" Shots: %d (outliers removed: %d)"),
|
||||
LastCalibrationResult.TotalShots, LastCalibrationResult.OutliersRemoved));
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalGood,
|
||||
FString::Printf(TEXT(" Corruption: Med %.3fs | P95 %.3fs | Max %.3fs"),
|
||||
LastCalibrationResult.MedianCorruptionDuration,
|
||||
LastCalibrationResult.P95CorruptionDuration,
|
||||
LastCalibrationResult.MaxCorruptionDuration));
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalGood,
|
||||
FString::Printf(TEXT(" Peak deviations: %.2f deg | %.2f cm"),
|
||||
LastCalibrationResult.AvgPeakAngleDeviation,
|
||||
LastCalibrationResult.AvgPeakPositionDeviation));
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalWarn,
|
||||
FString::Printf(TEXT(" >> DiscardTime: %.4fs"),
|
||||
LastCalibrationResult.RecommendedDiscardTime));
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalWarn,
|
||||
FString::Printf(TEXT(" >> BufferTime: %.4fs"),
|
||||
LastCalibrationResult.RecommendedBufferTime));
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalWarn,
|
||||
FString::Printf(TEXT(" >> KalmanProcessNoise: %.3f"),
|
||||
LastCalibrationResult.RecommendedKalmanProcessNoise));
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalWarn,
|
||||
FString::Printf(TEXT(" >> KalmanMeasurementNoise: %.4f"),
|
||||
LastCalibrationResult.RecommendedKalmanMeasurementNoise));
|
||||
}
|
||||
|
||||
GEngine->AddOnScreenDebugMessage(CalKey--, 0.0f, CalTitle,
|
||||
TEXT("================================="));
|
||||
}
|
||||
|
||||
//Only server can tick
|
||||
@ -233,6 +405,39 @@ void UEBBarrel::SpawnBullet(AActor* Owner, FVector InLocation, FVector InAim, in
|
||||
}
|
||||
}
|
||||
|
||||
// Capture yellow debug line on real shot: shows raw tracker aim at firing moment
|
||||
// (where the bullet would go WITHOUT anti-recoil correction)
|
||||
if (DebugAntiRecoil && TransformHistory.Num() > 0)
|
||||
{
|
||||
DebugIMUShockCapturedLocation = TransformHistory.Last().Location;
|
||||
DebugIMUShockCapturedAim = TransformHistory.Last().Aim;
|
||||
DebugIMUShockLineCaptured = true;
|
||||
DebugIMUShockLineEndTime = GetWorld()->GetTimeSeconds() + DebugIMUShockDisplayTime;
|
||||
|
||||
// Blue line: corrected aim direction retained for this shot (after anti-recoil filtering)
|
||||
DebugCorrectedShotCapturedLocation = OutLocation;
|
||||
DebugCorrectedShotCapturedAim = OutAim.GetSafeNormal();
|
||||
DebugCorrectedShotLineCaptured = true;
|
||||
DebugCorrectedShotLineEndTime = GetWorld()->GetTimeSeconds() + DebugIMUShockDisplayTime;
|
||||
}
|
||||
|
||||
// Calibration: snapshot the entire buffer for offline analysis
|
||||
if (CalibrateAntiRecoil && TransformHistory.Num() > 0)
|
||||
{
|
||||
FCalibrationShotData ShotData;
|
||||
ShotData.BufferSnapshot = TransformHistory;
|
||||
ShotData.ShotTime = GetWorld()->GetTimeSeconds();
|
||||
CalibrationShots.Add(MoveTemp(ShotData));
|
||||
CalibrationShotsCollected++;
|
||||
|
||||
if (CalibrationShotsCollected >= CalibrationShotCount)
|
||||
{
|
||||
ComputeCalibrationResult();
|
||||
CalibrationShots.Empty();
|
||||
CalibrationShotsCollected = 0;
|
||||
}
|
||||
}
|
||||
|
||||
BeforeShotFired.Broadcast();
|
||||
#ifdef WITH_EDITOR
|
||||
if (shotTrace) {
|
||||
@ -314,4 +519,260 @@ void UEBBarrel::ApplyRecoil_Implementation(UPrimitiveComponent* Component, FVect
|
||||
if (Component->IsSimulatingPhysics()) {
|
||||
Component->AddImpulseAtLocation(Impulse, InLocation);
|
||||
}
|
||||
}
|
||||
|
||||
void UEBBarrel::ComputeCalibrationResult()
|
||||
{
|
||||
LastCalibrationResult = FCalibrationResult();
|
||||
LastCalibrationResult.TotalShots = CalibrationShots.Num();
|
||||
|
||||
if (CalibrationShots.Num() < 3)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
TArray<float> CorruptionDurations;
|
||||
TArray<float> PeakAngles;
|
||||
TArray<float> PeakPositions;
|
||||
|
||||
// Collect all clean-sample acceleration variances and residuals for Kalman estimation
|
||||
TArray<float> AllAccelerationVariances;
|
||||
TArray<float> AllResidualVariances;
|
||||
|
||||
for (const FCalibrationShotData& Shot : CalibrationShots)
|
||||
{
|
||||
const TArray<FTimestampedTransform>& Buffer = Shot.BufferSnapshot;
|
||||
if (Buffer.Num() < 5)
|
||||
{
|
||||
CorruptionDurations.Add(0.0f);
|
||||
PeakAngles.Add(0.0f);
|
||||
PeakPositions.Add(0.0f);
|
||||
continue;
|
||||
}
|
||||
|
||||
// --- Auto-seuil 3-sigma corruption detection ---
|
||||
// Baseline: 30% oldest samples (min 3)
|
||||
int32 BaselineCount = FMath::Max(3, Buffer.Num() * 3 / 10);
|
||||
|
||||
// Fit linear trend on baseline (position and aim vs time)
|
||||
double T0 = Buffer[0].Timestamp;
|
||||
|
||||
// Weighted means for linear regression on baseline
|
||||
double SumT = 0.0, SumTT = 0.0;
|
||||
FVector SumPos = FVector::ZeroVector, SumTPos = FVector::ZeroVector;
|
||||
FVector SumAim = FVector::ZeroVector, SumTAim = FVector::ZeroVector;
|
||||
|
||||
for (int32 i = 0; i < BaselineCount; i++)
|
||||
{
|
||||
double t = Buffer[i].Timestamp - T0;
|
||||
SumT += t;
|
||||
SumTT += t * t;
|
||||
SumPos += Buffer[i].Location;
|
||||
SumTPos += Buffer[i].Location * t;
|
||||
SumAim += Buffer[i].Aim;
|
||||
SumTAim += Buffer[i].Aim * t;
|
||||
}
|
||||
|
||||
double N = (double)BaselineCount;
|
||||
double Det = N * SumTT - SumT * SumT;
|
||||
|
||||
FVector PosIntercept, PosSlope, AimIntercept, AimSlope;
|
||||
if (FMath::Abs(Det) > SMALL_NUMBER)
|
||||
{
|
||||
PosIntercept = (SumPos * SumTT - SumTPos * SumT) / Det;
|
||||
PosSlope = (SumTPos * N - SumPos * SumT) / Det;
|
||||
AimIntercept = (SumAim * SumTT - SumTAim * SumT) / Det;
|
||||
AimSlope = (SumTAim * N - SumAim * SumT) / Det;
|
||||
}
|
||||
else
|
||||
{
|
||||
PosIntercept = SumPos / N;
|
||||
PosSlope = FVector::ZeroVector;
|
||||
AimIntercept = SumAim / N;
|
||||
AimSlope = FVector::ZeroVector;
|
||||
}
|
||||
|
||||
// Compute residuals on baseline to get sigma
|
||||
float SumAngleResidSq = 0.0f;
|
||||
float SumPosResidSq = 0.0f;
|
||||
|
||||
for (int32 i = 0; i < BaselineCount; i++)
|
||||
{
|
||||
double t = Buffer[i].Timestamp - T0;
|
||||
FVector PredAim = (AimIntercept + AimSlope * t).GetSafeNormal();
|
||||
FVector PredPos = PosIntercept + PosSlope * t;
|
||||
|
||||
float Dot = FVector::DotProduct(Buffer[i].Aim.GetSafeNormal(), PredAim);
|
||||
float AngleResid = FMath::RadiansToDegrees(FMath::Acos(FMath::Clamp(Dot, -1.0f, 1.0f)));
|
||||
float PosResid = FVector::Dist(Buffer[i].Location, PredPos);
|
||||
|
||||
SumAngleResidSq += AngleResid * AngleResid;
|
||||
SumPosResidSq += PosResid * PosResid;
|
||||
}
|
||||
|
||||
float SigmaAngle = FMath::Sqrt(SumAngleResidSq / FMath::Max(1.0f, N - 2.0f));
|
||||
float SigmaPos = FMath::Sqrt(SumPosResidSq / FMath::Max(1.0f, N - 2.0f));
|
||||
|
||||
// Minimum sigma to avoid zero-threshold (perfectly still tracker)
|
||||
SigmaAngle = FMath::Max(SigmaAngle, 0.05f);
|
||||
SigmaPos = FMath::Max(SigmaPos, 0.01f);
|
||||
|
||||
float AngleThreshold = SigmaAngle * 3.0f;
|
||||
float PosThreshold = SigmaPos * 3.0f;
|
||||
|
||||
// Detect corruption in all samples after baseline
|
||||
float ShotCorruptionDuration = 0.0f;
|
||||
float ShotPeakAngle = 0.0f;
|
||||
float ShotPeakPos = 0.0f;
|
||||
double FirstCorruptedTime = 0.0;
|
||||
|
||||
for (int32 i = BaselineCount; i < Buffer.Num(); i++)
|
||||
{
|
||||
double t = Buffer[i].Timestamp - T0;
|
||||
FVector PredAim = (AimIntercept + AimSlope * t).GetSafeNormal();
|
||||
FVector PredPos = PosIntercept + PosSlope * t;
|
||||
|
||||
float Dot = FVector::DotProduct(Buffer[i].Aim.GetSafeNormal(), PredAim);
|
||||
float AngleDev = FMath::RadiansToDegrees(FMath::Acos(FMath::Clamp(Dot, -1.0f, 1.0f)));
|
||||
float PosDev = FVector::Dist(Buffer[i].Location, PredPos);
|
||||
|
||||
if (AngleDev > AngleThreshold || PosDev > PosThreshold)
|
||||
{
|
||||
if (FirstCorruptedTime == 0.0)
|
||||
{
|
||||
FirstCorruptedTime = Buffer[i].Timestamp;
|
||||
}
|
||||
if (AngleDev > ShotPeakAngle) ShotPeakAngle = AngleDev;
|
||||
if (PosDev > ShotPeakPos) ShotPeakPos = PosDev;
|
||||
}
|
||||
}
|
||||
|
||||
if (FirstCorruptedTime > 0.0)
|
||||
{
|
||||
ShotCorruptionDuration = (float)(Shot.ShotTime - FirstCorruptedTime);
|
||||
}
|
||||
|
||||
CorruptionDurations.Add(ShotCorruptionDuration);
|
||||
PeakAngles.Add(ShotPeakAngle);
|
||||
PeakPositions.Add(ShotPeakPos);
|
||||
|
||||
// --- Kalman parameter estimation from clean baseline samples ---
|
||||
// ProcessNoise: variance of acceleration (velocity changes between consecutive samples)
|
||||
if (BaselineCount >= 3)
|
||||
{
|
||||
float SumAccelSq = 0.0f;
|
||||
int32 AccelCount = 0;
|
||||
FVector PrevVel = FVector::ZeroVector;
|
||||
bool bHasPrevVel = false;
|
||||
|
||||
for (int32 i = 1; i < BaselineCount; i++)
|
||||
{
|
||||
double dt = Buffer[i].Timestamp - Buffer[i - 1].Timestamp;
|
||||
if (dt > SMALL_NUMBER)
|
||||
{
|
||||
FVector Vel = (Buffer[i].Location - Buffer[i - 1].Location) / dt;
|
||||
if (bHasPrevVel)
|
||||
{
|
||||
FVector Accel = (Vel - PrevVel) / dt;
|
||||
SumAccelSq += Accel.SizeSquared();
|
||||
AccelCount++;
|
||||
}
|
||||
PrevVel = Vel;
|
||||
bHasPrevVel = true;
|
||||
}
|
||||
}
|
||||
if (AccelCount > 0)
|
||||
{
|
||||
AllAccelerationVariances.Add(SumAccelSq / AccelCount);
|
||||
}
|
||||
}
|
||||
|
||||
// MeasurementNoise: variance of residuals from linear trend
|
||||
if (SumPosResidSq > 0.0f)
|
||||
{
|
||||
AllResidualVariances.Add(SumPosResidSq / FMath::Max(1.0f, N - 2.0f));
|
||||
}
|
||||
}
|
||||
|
||||
// --- Aggregate timing statistics ---
|
||||
CorruptionDurations.Sort();
|
||||
int32 Num = CorruptionDurations.Num();
|
||||
|
||||
// IQR outlier removal
|
||||
float Q1 = CorruptionDurations[Num / 4];
|
||||
float Q3 = CorruptionDurations[(3 * Num) / 4];
|
||||
float IQR = Q3 - Q1;
|
||||
float UpperFence = Q3 + 1.5f * IQR;
|
||||
|
||||
TArray<float> CleanDurations;
|
||||
int32 OutlierCount = 0;
|
||||
for (float D : CorruptionDurations)
|
||||
{
|
||||
if (D <= UpperFence)
|
||||
CleanDurations.Add(D);
|
||||
else
|
||||
OutlierCount++;
|
||||
}
|
||||
|
||||
if (CleanDurations.Num() < 3)
|
||||
{
|
||||
CleanDurations = CorruptionDurations;
|
||||
OutlierCount = 0;
|
||||
}
|
||||
|
||||
CleanDurations.Sort();
|
||||
int32 CN = CleanDurations.Num();
|
||||
|
||||
float Median = CleanDurations[CN / 2];
|
||||
int32 P95Index = FMath::Min((int32)(CN * 0.95f), CN - 1);
|
||||
float P95 = CleanDurations[P95Index];
|
||||
float Max = CleanDurations.Last();
|
||||
|
||||
float RecommendedDiscard = P95 * 1.3f;
|
||||
RecommendedDiscard = FMath::Clamp(RecommendedDiscard, 0.011f, 0.2f);
|
||||
|
||||
float SafeWindow = FMath::Max(0.05f, RecommendedDiscard * 0.5f);
|
||||
float RecommendedBuffer = RecommendedDiscard + SafeWindow;
|
||||
|
||||
// Average peak deviations
|
||||
float SumAngle = 0.0f, SumPos = 0.0f;
|
||||
for (int32 i = 0; i < PeakAngles.Num(); i++)
|
||||
{
|
||||
SumAngle += PeakAngles[i];
|
||||
SumPos += PeakPositions[i];
|
||||
}
|
||||
|
||||
// --- Kalman parameter recommendations ---
|
||||
float RecommendedProcessNoise = 200.0f; // default fallback
|
||||
float RecommendedMeasurementNoise = 0.01f; // default fallback
|
||||
|
||||
if (AllAccelerationVariances.Num() > 0)
|
||||
{
|
||||
float SumAccelVar = 0.0f;
|
||||
for (float V : AllAccelerationVariances) SumAccelVar += V;
|
||||
RecommendedProcessNoise = SumAccelVar / AllAccelerationVariances.Num();
|
||||
// Clamp to sane range
|
||||
RecommendedProcessNoise = FMath::Clamp(RecommendedProcessNoise, 0.1f, 10000.0f);
|
||||
}
|
||||
|
||||
if (AllResidualVariances.Num() > 0)
|
||||
{
|
||||
float SumResidVar = 0.0f;
|
||||
for (float V : AllResidualVariances) SumResidVar += V;
|
||||
RecommendedMeasurementNoise = SumResidVar / AllResidualVariances.Num();
|
||||
RecommendedMeasurementNoise = FMath::Clamp(RecommendedMeasurementNoise, 0.001f, 100.0f);
|
||||
}
|
||||
|
||||
// Populate result
|
||||
LastCalibrationResult.RecommendedDiscardTime = RecommendedDiscard;
|
||||
LastCalibrationResult.RecommendedBufferTime = RecommendedBuffer;
|
||||
LastCalibrationResult.RecommendedKalmanProcessNoise = RecommendedProcessNoise;
|
||||
LastCalibrationResult.RecommendedKalmanMeasurementNoise = RecommendedMeasurementNoise;
|
||||
LastCalibrationResult.MedianCorruptionDuration = Median;
|
||||
LastCalibrationResult.P95CorruptionDuration = P95;
|
||||
LastCalibrationResult.MaxCorruptionDuration = Max;
|
||||
LastCalibrationResult.AvgPeakAngleDeviation = SumAngle / PeakAngles.Num();
|
||||
LastCalibrationResult.AvgPeakPositionDeviation = SumPos / PeakPositions.Num();
|
||||
LastCalibrationResult.OutliersRemoved = OutlierCount;
|
||||
LastCalibrationResult.bValid = true;
|
||||
}
|
||||
@ -28,8 +28,10 @@ enum class EAntiRecoilMode : uint8
|
||||
ARM_None UMETA(DisplayName = "Disabled", ToolTip = "No anti-recoil processing. Uses raw tracker data directly. Use this when no IMU shock compensation is needed."),
|
||||
ARM_Buffer UMETA(DisplayName = "Buffer (No Prediction)", ToolTip = "Legacy mode. Returns the oldest sample in the buffer, guaranteed to be pre-shock. Simple and reliable but introduces a fixed time delay equal to BufferTime. Best for static or slow-moving aiming."),
|
||||
ARM_LinearExtrapolation UMETA(DisplayName = "Linear Extrapolation", ToolTip = "Computes average linear and angular velocity from consecutive safe (pre-shock) samples, then extrapolates forward to the current time. Good balance of simplicity and accuracy for steady movements. May overshoot on sudden direction changes."),
|
||||
ARM_WeightedRegression UMETA(DisplayName = "Weighted Regression", ToolTip = "Fits a weighted least-squares regression line through all safe samples (recent safe samples weighted higher), then extrapolates to current time. More robust to individual noisy samples than linear extrapolation. Slightly heavier computation."),
|
||||
ARM_KalmanFilter UMETA(DisplayName = "Kalman Filter", ToolTip = "Maintains an internal state model (position + velocity, aim + angular velocity) updated only with safe samples. Predicts forward using the estimated dynamics. Best for smooth continuous tracking with optimal noise rejection. Requires tuning ProcessNoise and MeasurementNoise for best results.")
|
||||
ARM_WeightedLinearRegression UMETA(DisplayName = "Weighted Linear Regression", ToolTip = "Fits a weighted least-squares line (y=a+bt) through safe samples. Recent samples weighted higher (controlled by RegressionWeightExponent). Simple, stable, no oscillation. May overshoot on sudden stops since it assumes constant velocity."),
|
||||
ARM_WeightedRegression UMETA(DisplayName = "Weighted Quadratic Regression", ToolTip = "Fits a weighted quadratic curve (y=a+bt+ct^2) through safe samples, capturing deceleration naturally. Blends smoothly between linear and quadratic based on acceleration significance. Falls back to linear with < 3 samples. Includes velocity-reversal clamping."),
|
||||
ARM_KalmanFilter UMETA(DisplayName = "Kalman Filter", ToolTip = "Maintains an internal state model (position + velocity, aim + angular velocity) updated only with safe samples. Predicts forward using the estimated dynamics. Best for smooth continuous tracking with optimal noise rejection. Requires tuning ProcessNoise and MeasurementNoise for best results."),
|
||||
ARM_AdaptiveExtrapolation UMETA(DisplayName = "Adaptive Extrapolation", ToolTip = "Computes weighted average velocity from safe samples, then scales extrapolation by a confidence factor based on velocity consistency. When velocity is steady (low variance), full extrapolation. When velocity changes rapidly (deceleration/direction change), extrapolation is reduced automatically. One tuning parameter: AdaptiveSensitivity.")
|
||||
};
|
||||
|
||||
USTRUCT()
|
||||
@ -42,6 +44,33 @@ struct FTimestampedTransform
|
||||
FVector Aim = FVector::ForwardVector;
|
||||
};
|
||||
|
||||
// Stores calibration measurement results from a sequence of shots
|
||||
USTRUCT()
|
||||
struct FCalibrationResult
|
||||
{
|
||||
GENERATED_BODY()
|
||||
|
||||
float RecommendedDiscardTime = 0.0f;
|
||||
float RecommendedBufferTime = 0.0f;
|
||||
float RecommendedKalmanProcessNoise = 0.0f;
|
||||
float RecommendedKalmanMeasurementNoise = 0.0f;
|
||||
float MedianCorruptionDuration = 0.0f;
|
||||
float P95CorruptionDuration = 0.0f;
|
||||
float MaxCorruptionDuration = 0.0f;
|
||||
float AvgPeakAngleDeviation = 0.0f;
|
||||
float AvgPeakPositionDeviation = 0.0f;
|
||||
int32 TotalShots = 0;
|
||||
int32 OutliersRemoved = 0;
|
||||
bool bValid = false;
|
||||
};
|
||||
|
||||
// Raw buffer snapshot captured at shot time for offline analysis
|
||||
struct FCalibrationShotData
|
||||
{
|
||||
TArray<FTimestampedTransform> BufferSnapshot;
|
||||
double ShotTime = 0.0;
|
||||
};
|
||||
|
||||
UCLASS(Blueprintable, ClassGroup = (Custom), hidecategories = (Object, LOD, Physics, Lighting, TextureStreaming, Collision, HLOD, Mobile, VirtualTexture, ComponentReplication), editinlinenew, meta = (BlueprintSpawnableComponent))
|
||||
class EASYBALLISTICS_API UEBBarrel : public UPrimitiveComponent
|
||||
{
|
||||
@ -52,6 +81,8 @@ public:
|
||||
// Sets default values for this component's properties
|
||||
UEBBarrel();
|
||||
|
||||
virtual void BeginPlay() override;
|
||||
|
||||
// Called every frame
|
||||
virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
|
||||
|
||||
@ -65,10 +96,23 @@ public:
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Debug", meta = (ToolTip = "Draw real-time debug lines: Green = raw tracker, Red = anti-recoil predicted aim"))
|
||||
bool DebugAntiRecoil = false;
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Debug", meta = (ToolTip = "Length of the debug aim lines (cm)", EditCondition = "DebugAntiRecoil"))
|
||||
float DebugAntiRecoilLineLength = 200.0f;
|
||||
float DebugAntiRecoilLineLength = 400.0f;
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Debug", meta = (ToolTip = "Thickness of the debug aim lines", EditCondition = "DebugAntiRecoil"))
|
||||
float DebugAntiRecoilLineThickness = 0.0f;
|
||||
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Debug|Calibration", meta = (ToolTip = "When enabled, automatically measures recoil corruption over sequences of CalibrationShotCount shots. After each sequence, displays recommended DiscardTime, BufferTime, and Kalman parameters on HUD. Loops automatically until disabled. No thresholds needed — uses statistical 3-sigma auto-detection."))
|
||||
bool CalibrateAntiRecoil = false;
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Debug|Calibration", meta = (ToolTip = "Number of shots per calibration sequence.", EditCondition = "CalibrateAntiRecoil", ClampMin = "3", ClampMax = "50"))
|
||||
int32 CalibrationShotCount = 10;
|
||||
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Debug|CSV Recording", meta = (ToolTip = "Record per-frame prediction data to CSV for offline analysis. File saved to project Saved/Logs/ folder. Toggle off to stop and close the file."))
|
||||
bool RecordPredictionCSV = false;
|
||||
|
||||
// CSV recording state (not exposed)
|
||||
bool bCSVFileOpen = false;
|
||||
FString CSVFilePath;
|
||||
IFileHandle* CSVFileHandle = nullptr;
|
||||
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Debug|IMU Shock Simulation", meta = (ToolTip = "Enable IMU shock simulation for testing anti-recoil prediction without firing"))
|
||||
bool DebugSimulateIMUShock = false;
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Debug|IMU Shock Simulation", meta = (ToolTip = "Angular perturbation intensity in degrees", EditCondition = "DebugSimulateIMUShock", ClampMin = "0.0"))
|
||||
@ -89,8 +133,8 @@ public:
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "AntiRecoil", meta = (ToolTip = "Time window (seconds) of most recent samples to exclude as potentially contaminated by IMU recoil shock. The prediction algorithms only use samples older than this. Increase if the shock lasts longer. Safe window = BufferTime - DiscardTime.", ClampMin = "0.0"))
|
||||
float AntiRecoilDiscardTime = 0.03f;
|
||||
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "AntiRecoil", meta = (ToolTip = "Controls how the weight curve grows across safe samples in Weighted Regression mode. 1.0 = linear growth (default), >1.0 = recent samples weighted much more heavily (convex curve), <1.0 = more uniform weighting (concave curve), 0.0 = all samples weighted equally (unweighted regression). Formula: weight = pow(sampleIndex+1, exponent).", EditCondition = "AntiRecoilMode == EAntiRecoilMode::ARM_WeightedRegression", ClampMin = "0.0", ClampMax = "5.0"))
|
||||
float RegressionWeightExponent = 3.0f;
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "AntiRecoil", meta = (ToolTip = "Controls how the weight curve grows across safe samples in regression modes. 1.0 = linear growth, >1.0 = recent samples weighted much more heavily (convex curve), <1.0 = more uniform weighting (concave curve), 0.0 = all samples weighted equally. Formula: weight = pow(sampleIndex+1, exponent).", EditCondition = "AntiRecoilMode == EAntiRecoilMode::ARM_WeightedRegression || AntiRecoilMode == EAntiRecoilMode::ARM_WeightedLinearRegression", ClampMin = "0.0", ClampMax = "5.0"))
|
||||
float RegressionWeightExponent = 2.0f;
|
||||
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "AntiRecoil", meta = (ToolTip = "Kalman filter process noise (higher = more responsive to movement changes, lower = smoother). Since safe samples are already filtered by DiscardTime, this should be high enough to track aiming movements.", EditCondition = "AntiRecoilMode == EAntiRecoilMode::ARM_KalmanFilter", ClampMin = "0.01"))
|
||||
float KalmanProcessNoise = 200.0f;
|
||||
@ -98,6 +142,12 @@ public:
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "AntiRecoil", meta = (ToolTip = "Kalman filter measurement noise (higher = trusts model over measurements, lower = trusts measurements). Should be low since safe samples are clean.", EditCondition = "AntiRecoilMode == EAntiRecoilMode::ARM_KalmanFilter", ClampMin = "0.001"))
|
||||
float KalmanMeasurementNoise = 0.01f;
|
||||
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "AntiRecoil", meta = (ToolTip = "Controls how sensitive the adaptive extrapolation is to velocity changes. Higher = more aggressive reduction during velocity changes (less overshoot but more latency). Lower = more permissive extrapolation. Uses relative variance so values are scale-independent. Typical range: 50-200. Use the analyze_antirecoil.py tool to find the optimal value for your setup.", EditCondition = "AntiRecoilMode == EAntiRecoilMode::ARM_AdaptiveExtrapolation", ClampMin = "0.01", ClampMax = "500.0"))
|
||||
float AdaptiveSensitivity = 100.0f;
|
||||
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "AntiRecoil", meta = (ToolTip = "Velocity damping during extrapolation. 0 = disabled (default). Higher values cause extrapolated velocity to decay exponentially toward zero over the discard window. Reduces overshoot on fast draw-aim-fire sequences where the user stops moving before firing. Applies to all prediction modes except Buffer. Typical range: 5-15.", ClampMin = "0.0", ClampMax = "50.0"))
|
||||
float ExtrapolationDamping = 0.0f;
|
||||
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Velocity", meta = (ToolTip = "Bullet inherits barrel velocity, only works with physics enabled or with additional velocity set")) float InheritVelocity = 1.0f;
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Velocity", meta = (ToolTip = "Amount of recoil applied to the barrel, only works with physics enabled")) float RecoilMultiplier = 1.0f;
|
||||
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category = "Velocity", meta = (ToolTip = "Additional velocity, for use with InheritVelocity")) FVector AdditionalVelocity = FVector(0,0,0);
|
||||
@ -234,12 +284,24 @@ private:
|
||||
FVector DebugIMUShockAimOffset = FVector::ZeroVector;
|
||||
FVector DebugIMUShockPosOffset = FVector::ZeroVector;
|
||||
|
||||
// Debug yellow line persistence (shows uncorrected aim after shock)
|
||||
// Debug yellow line persistence (shows uncorrected raw aim after shock)
|
||||
bool DebugIMUShockLineCaptured = false;
|
||||
double DebugIMUShockLineEndTime = 0.0;
|
||||
FVector DebugIMUShockCapturedLocation = FVector::ZeroVector;
|
||||
FVector DebugIMUShockCapturedAim = FVector::ForwardVector;
|
||||
|
||||
// Debug blue line persistence (shows corrected aim retained for the shot)
|
||||
bool DebugCorrectedShotLineCaptured = false;
|
||||
double DebugCorrectedShotLineEndTime = 0.0;
|
||||
FVector DebugCorrectedShotCapturedLocation = FVector::ZeroVector;
|
||||
FVector DebugCorrectedShotCapturedAim = FVector::ForwardVector;
|
||||
|
||||
// Calibration state
|
||||
int32 CalibrationShotsCollected = 0;
|
||||
TArray<FCalibrationShotData> CalibrationShots;
|
||||
FCalibrationResult LastCalibrationResult;
|
||||
void ComputeCalibrationResult();
|
||||
|
||||
// Kalman filter state
|
||||
FVector KalmanPosition = FVector::ZeroVector;
|
||||
FVector KalmanVelocity = FVector::ZeroVector;
|
||||
@ -255,7 +317,9 @@ private:
|
||||
void UpdateTransformHistory();
|
||||
void ComputeAntiRecoilTransform();
|
||||
void PredictLinearExtrapolation(double CurrentTime, FVector& OutLocation, FVector& OutAim) const;
|
||||
void PredictWeightedLinearRegression(double CurrentTime, FVector& OutLocation, FVector& OutAim) const;
|
||||
void PredictWeightedRegression(double CurrentTime, FVector& OutLocation, FVector& OutAim) const;
|
||||
void PredictAdaptiveExtrapolation(double CurrentTime, FVector& OutLocation, FVector& OutAim) const;
|
||||
void UpdateKalmanFilter(double CurrentTime, const FVector& MeasuredLocation, const FVector& MeasuredAim);
|
||||
void PredictKalmanFilter(double CurrentTime, FVector& OutLocation, FVector& OutAim) const;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user