From 2b3bdd6e2504ed0db158bfb602842c4b073da9b9 Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Thu, 9 Apr 2026 15:59:28 -0400 Subject: [PATCH 01/10] Actually benchmark the airbrake math --- .../tools/benchmark_airbrake_math.cpp | 318 ++++++++++++++++++ 1 file changed, 318 insertions(+) create mode 100644 app/other/airbraker/tools/benchmark_airbrake_math.cpp diff --git a/app/other/airbraker/tools/benchmark_airbrake_math.cpp b/app/other/airbraker/tools/benchmark_airbrake_math.cpp new file mode 100644 index 000000000..241422271 --- /dev/null +++ b/app/other/airbraker/tools/benchmark_airbrake_math.cpp @@ -0,0 +1,318 @@ +#include "../src/math/c_manualmatrix.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + +using Matrix3 = ManualMatrix<3, 3, float>; +using Vector3 = ManualMatrix<3, 1, float>; + +constexpr std::array kAtmosphere = { + 1.2345e2F, -1.876e-2F, 3.4e-6F, -5.1e-10F, 7.2e-14F, -3.3e-18F, 6.8e-23F, +}; + +constexpr std::array, 3> kImuToRocket = {{ + {0.9961947F, -0.0121320F, 0.0864101F}, + {0.0151344F, 0.9993007F, -0.0336292F}, + {-0.0858317F, 0.0349465F, 0.9956955F}, +}}; + +template +inline void DoNotOptimize(T const& value) +{ + asm volatile("" : : "g"(value) : "memory"); +} + +float ClampUnit(float v) +{ + return std::max(-1.0F, std::min(1.0F, v)); +} + +float Dot3(const Vector3& a, const Vector3& b) +{ + return (a.Get(0, 0) * b.Get(0, 0)) + (a.Get(1, 0) * b.Get(1, 0)) + (a.Get(2, 0) * b.Get(2, 0)); +} + +std::array RotateImuToRocket(const std::array& v) +{ + std::array out{}; + for (int r = 0; r < 3; ++r) + { + out[r] = (kImuToRocket[r][0] * v[0]) + (kImuToRocket[r][1] * v[1]) + (kImuToRocket[r][2] * v[2]); + } + return out; +} + +std::array RotateRocketToImu(const std::array& v) +{ + std::array out{}; + for (int r = 0; r < 3; ++r) + { + out[r] = (kImuToRocket[0][r] * v[0]) + (kImuToRocket[1][r] * v[1]) + (kImuToRocket[2][r] * v[2]); + } + return out; +} + +Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) +{ + w1 *= t; + w2 *= t; + w3 *= t; + + const float w1_2 = w1 * w1; + const float w2_2 = w2 * w2; + const float w3_2 = w3 * w3; + const float w1w2 = w1 * w2; + const float w1w3 = w1 * w3; + const float w2w3 = w2 * w3; + + Matrix3 A{{ + 0, -w3, w2, + w3, 0, -w1, + -w2, w1, 0, + }}; + Matrix3 A2{{ + -(w2_2 + w3_2), w1w2, w1w3, + w1w2, -(w1_2 + w3_2), w2w3, + w1w3, w2w3, -(w1_2 + w2_2), + }}; + + const float normSq = w1_2 + w2_2 + w3_2; + const float norm = std::sqrt(normSq); + const float s = (norm == 0.0F) ? 1.0F : (std::sin(norm) / norm); + const float c = (norm == 0.0F) ? 0.0F : ((1.0F - std::cos(norm)) / normSq); + + return Matrix3::Identity() + (A * s) + (A2 * c); +} + +Matrix3 ExpGyroNew(float w1, float w2, float w3, float t) +{ + const float wx = w1 * t; + const float wy = w2 * t; + const float wz = w3 * t; + + const float wx2 = wx * wx; + const float wy2 = wy * wy; + const float wz2 = wz * wz; + const float wxwy = wx * wy; + const float wxwz = wx * wz; + const float wywz = wy * wz; + + const float thetaSq = wx2 + wy2 + wz2; + const float theta = std::sqrt(thetaSq); + const float s = (theta == 0.0F) ? 1.0F : (std::sin(theta) / theta); + const float c = (theta == 0.0F) ? 0.0F : ((1.0F - std::cos(theta)) / thetaSq); + + return Matrix3{{ + 1.0F - c * (wy2 + wz2), c * wxwy - s * wz, c * wxwz + s * wy, + c * wxwy + s * wz, 1.0F - c * (wx2 + wz2), c * wywz - s * wx, + c * wxwz - s * wy, c * wywz + s * wx, 1.0F - c * (wx2 + wy2), + }}; +} + +float AltitudeOriginal(float kPa) +{ + const float x = kPa * 1000.0F; + float sum = 0.0F; + float xn = x; + for (std::size_t i = 1; i < kAtmosphere.size(); ++i) + { + sum += kAtmosphere[i] * xn; + xn *= x; + } + return sum + kAtmosphere[0]; +} + +float AltitudeNew(float kPa) +{ + const float x = kPa * 1000.0F; + float sum = kAtmosphere[kAtmosphere.size() - 1]; + for (std::size_t i = kAtmosphere.size() - 1; i-- > 0;) + { + sum = (sum * x) + kAtmosphere[i]; + } + return sum; +} + +struct GyroStepResult { + Matrix3 orientation; + bool outOfBounds; +}; + +GyroStepResult FeedGyroOriginal(Matrix3 orientation, float gx, float gy, float gz, float dt) +{ + const Matrix3 eAT = ExpGyroOriginal(gx, gy, gz, dt); + orientation = orientation * eAT; + + const auto zInImu = RotateRocketToImu({0.0F, 0.0F, 1.0F}); + const Vector3 initial{{zInImu[0], zInImu[1], zInImu[2]}}; + const Vector3 now = orientation * initial; + const auto rotated = RotateImuToRocket({now.Get(0, 0), now.Get(1, 0), now.Get(2, 0)}); + + (void)rotated; + + const float initialNorm = std::sqrt((initial.Get(0, 0) * initial.Get(0, 0)) + + (initial.Get(1, 0) * initial.Get(1, 0)) + + (initial.Get(2, 0) * initial.Get(2, 0))); + const Vector3 normedNow = initial * (1.0F / initialNorm); + const float dot = (initial.Get(0, 0) * normedNow.Get(0, 0)) + + (initial.Get(1, 0) * normedNow.Get(1, 0)) + + (initial.Get(2, 0) * normedNow.Get(2, 0)); + const float offStart = std::acos(dot); + return {orientation, offStart > (30.0F * 3.14159F / 180.0F)}; +} + +GyroStepResult FeedGyroNew(Matrix3 orientation, float gx, float gy, float gz, float dt) +{ + constexpr float kCosMaxTilt = 0.8660254037844386F; + static const Vector3 initial = [] { + const auto zInImu = RotateRocketToImu({0.0F, 0.0F, 1.0F}); + return Vector3{{zInImu[0], zInImu[1], zInImu[2]}}; + }(); + + const Matrix3 eAT = ExpGyroNew(gx, gy, gz, dt); + orientation = orientation * eAT; + + const Vector3 now = orientation * initial; + const float nowNorm = std::sqrt(Dot3(now, now)); + const float cosOffStart = (nowNorm == 0.0F) ? 1.0F : ClampUnit(Dot3(initial, now) / nowNorm); + return {orientation, cosOffStart < kCosMaxTilt}; +} + +struct Stats { + double minNs; + double meanNs; +}; + +template +Stats RunBenchmark(const char* name, Fn&& fn, std::size_t iterations, std::size_t rounds) +{ + std::vector samples; + samples.reserve(rounds); + for (std::size_t round = 0; round < rounds; ++round) + { + const auto start = std::chrono::steady_clock::now(); + fn(iterations); + const auto end = std::chrono::steady_clock::now(); + const double elapsedNs = std::chrono::duration(end - start).count(); + samples.push_back(elapsedNs / static_cast(iterations)); + } + + const double minNs = *std::min_element(samples.begin(), samples.end()); + const double meanNs = std::accumulate(samples.begin(), samples.end(), 0.0) / static_cast(samples.size()); + std::printf("%-24s min %.2f ns/op mean %.2f ns/op\n", name, minNs, meanNs); + return {minNs, meanNs}; +} + +} // namespace + +int main() +{ + constexpr std::size_t kIterations = 1'000'000; + constexpr std::size_t kRounds = 8; + + volatile float sink = 0.0F; + volatile bool sinkBool = false; + + for (std::size_t i = 0; i < 10000; ++i) + { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); + sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); + sink += AltitudeOriginal(85.0F + static_cast(i % 50) * 0.1F); + sink += AltitudeNew(85.0F + static_cast(i % 50) * 0.1F); + } + + std::puts("Airbraker math benchmark"); + std::printf("iterations=%zu rounds=%zu\n\n", kIterations, kRounds); + + const auto expOriginal = RunBenchmark("expGyro original", [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) + { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); + } + DoNotOptimize(sink); + }, kIterations, kRounds); + + const auto expNew = RunBenchmark("expGyro new", [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) + { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); + } + DoNotOptimize(sink); + }, kIterations, kRounds); + + const auto altOriginal = RunBenchmark("altitude original", [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) + { + sink += AltitudeOriginal(80.0F + static_cast(i % 1000) * 0.02F); + } + DoNotOptimize(sink); + }, kIterations, kRounds); + + const auto altNew = RunBenchmark("altitude new", [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) + { + sink += AltitudeNew(80.0F + static_cast(i % 1000) * 0.02F); + } + DoNotOptimize(sink); + }, kIterations, kRounds); + + const auto feedOriginal = RunBenchmark("feedGyro original", [&](std::size_t iterations) { + Matrix3 orientation = Matrix3::Identity(); + bool everOut = false; + for (std::size_t i = 0; i < iterations; ++i) + { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + const auto result = FeedGyroOriginal(orientation, gx, gy, gz, 0.01F); + orientation = result.orientation; + everOut |= result.outOfBounds; + } + sink += orientation.Get(0, 0); + sinkBool ^= everOut; + DoNotOptimize(sink); + DoNotOptimize(sinkBool); + }, kIterations, kRounds); + + const auto feedNew = RunBenchmark("feedGyro new", [&](std::size_t iterations) { + Matrix3 orientation = Matrix3::Identity(); + bool everOut = false; + for (std::size_t i = 0; i < iterations; ++i) + { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + const auto result = FeedGyroNew(orientation, gx, gy, gz, 0.01F); + orientation = result.orientation; + everOut |= result.outOfBounds; + } + sink += orientation.Get(0, 0); + sinkBool ^= everOut; + DoNotOptimize(sink); + DoNotOptimize(sinkBool); + }, kIterations, kRounds); + + std::puts("\nSpeedups based on best run"); + std::printf("expGyro: %.2fx\n", expOriginal.minNs / expNew.minNs); + std::printf("altitude: %.2fx\n", altOriginal.minNs / altNew.minNs); + std::printf("feedGyro step: %.2fx\n", feedOriginal.minNs / feedNew.minNs); + std::printf("\nignore sinks: %f %d\n", sink, static_cast(sinkBool)); + return 0; +} From 7b975a5a9e1c419184e4a1aed08427c92fee90ce Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Thu, 9 Apr 2026 16:08:11 -0400 Subject: [PATCH 02/10] Make expGyro less silly --- app/other/airbraker/src/n_model.cpp | 55 +++++++++++------------------ 1 file changed, 21 insertions(+), 34 deletions(-) diff --git a/app/other/airbraker/src/n_model.cpp b/app/other/airbraker/src/n_model.cpp index 029daa093..87458389c 100644 --- a/app/other/airbraker/src/n_model.cpp +++ b/app/other/airbraker/src/n_model.cpp @@ -89,40 +89,27 @@ float AltitudeMetersFromPressureKPa(float pressure_kpa) Matrix<3, 3> expGyro(float w_1, float w_2, float w_3, float t) { - w_1 *= t; - w_2 *= t; - w_3 *= t; - - float w_1² = w_1 * w_1; - float w_2² = w_2 * w_2; - float w_3² = w_3 * w_3; - float w_1w_2 = w_1 * w_2; - float w_1w_3 = w_1 * w_3; - float w_2w_3 = w_2 * w_3; - // clang-format off - Matrix<3,3> A{{ - 0, -w_3, w_2, - w_3, 0, -w_1, - -w_2, w_1, 0, - }}; - Matrix<3,3> A²{{ - -(w_2² + w_3²), w_1w_2, w_1w_3, - w_1w_2, -(w_1²+w_3²), w_2w_3, - w_1w_3, w_2w_3, -(w_1²+w_2²), - }}; - - // clang-format on - float norm_sqred = w_1² + w_2² + w_3²; - float norm = std::sqrt(norm_sqred); - - // proof via desmos, this is what happens. (sinx/x = 1 (1-cosx)/x = 0 ) - float s = (norm == 0) ? 1 : (std::sin(norm) / norm); - float c = (norm == 0) ? 0 : ((1 - std::cos(norm)) / (norm_sqred)); - - Matrix<3, 3> I = Matrix<3, 3>::Identity(); - - auto eᴬᵗ = I + A * s + A² * c; - return eᴬᵗ; + const float wx = w_1 * t; + const float wy = w_2 * t; + const float wz = w_3 * t; + + const float wx2 = wx * wx; + const float wy2 = wy * wy; + const float wz2 = wz * wz; + const float wxwy = wx * wy; + const float wxwz = wx * wz; + const float wywz = wy * wz; + + const float thetaSq = wx2 + wy2 + wz2; + const float theta = std::sqrt(thetaSq); + const float s = (theta == 0.0F) ? 1.0F : (std::sin(theta) / theta); + const float c = (theta == 0.0F) ? 0.0F : ((1.0F - std::cos(theta)) / thetaSq); + + return Matrix<3, 3>{ { + 1.0F - c * (wy2 + wz2), c * wxwy - s * wz, c * wxwz + s * wy, + c * wxwy + s * wz, 1.0F - c * (wx2 + wz2), c * wywz - s * wx, + c * wxwz - s * wy, c * wywz + s * wx, 1.0F - c * (wx2 + wy2), + } }; } float deg2rad(float d) From fb019cb8027ef1f623371432b9894b4f207f427c Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Thu, 9 Apr 2026 16:11:25 -0400 Subject: [PATCH 03/10] Do the atmosphere polynomial the normal way --- app/other/airbraker/src/n_model.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/app/other/airbraker/src/n_model.cpp b/app/other/airbraker/src/n_model.cpp index 87458389c..fbaab2cfa 100644 --- a/app/other/airbraker/src/n_model.cpp +++ b/app/other/airbraker/src/n_model.cpp @@ -67,16 +67,13 @@ KalmanState LastKalmanState() #ifdef CUSTOM_ATMOSPHERE float AltitudeMetersFromPressureKPa(float kPa) { - float x = kPa * 1000; - float sum = 0; - float xn = x; - for (size_t i = 1; i < AUTOGEN_ATMOSPHERE_NUM_COEFFECIENTS; i++) + const float x = kPa * 1000.0F; + float sum = ATMOSPHERE[AUTOGEN_ATMOSPHERE_NUM_COEFFECIENTS - 1]; + for (size_t i = AUTOGEN_ATMOSPHERE_NUM_COEFFECIENTS - 1; i-- > 0;) { - const float coeff = ATMOSPHERE[i]; - sum += coeff * xn; - xn *= x; + sum = (sum * x) + ATMOSPHERE[i]; } - return sum + ATMOSPHERE[0]; + return sum; } #else float AltitudeMetersFromPressureKPa(float pressure_kpa) From d8eb80eb572b03215dae541974d56fa12ae8acb1 Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Thu, 9 Apr 2026 16:14:56 -0400 Subject: [PATCH 04/10] Make the gyro tilt check actually check tilt --- app/other/airbraker/src/n_model.cpp | 58 +++++++++++++++++++---------- 1 file changed, 38 insertions(+), 20 deletions(-) diff --git a/app/other/airbraker/src/n_model.cpp b/app/other/airbraker/src/n_model.cpp index fbaab2cfa..91c2b79f4 100644 --- a/app/other/airbraker/src/n_model.cpp +++ b/app/other/airbraker/src/n_model.cpp @@ -9,6 +9,39 @@ namespace NModel { +namespace +{ +constexpr float kCosMaxTilt = 0.8660254037844386F; + +float ClampUnit(float v) +{ + if (v > 1.0F) + { + return 1.0F; + } + if (v < -1.0F) + { + return -1.0F; + } + return v; +} + +const Matrix<3, 1>& InitialRocketZAxisInIMUSpace() +{ + static const Matrix<3, 1> initial = [] { + NTypes::AccelerometerData zInIMUSpace{}; + RotateRocketVectorToIMUVector({ 0, 0, 1 }, zInIMUSpace); + return Matrix<3, 1>{ { zInIMUSpace.X, zInIMUSpace.Y, zInIMUSpace.Z } }; + }(); + return initial; +} + +float Dot3(const Matrix<3, 1>& a, const Matrix<3, 1>& b) +{ + return (a.Get(0, 0) * b.Get(0, 0)) + (a.Get(1, 0) * b.Get(1, 0)) + (a.Get(2, 0) * b.Get(2, 0)); +} +} // namespace + const char* GetMatlabLUTName() { return LUT_NAME; @@ -130,26 +163,11 @@ void FeedGyro(uint32_t msSinceBoot, const NTypes::GyroscopeData& gyro) lastGyroIntegrationMsSinceBoot = msSinceBoot; Matrix<3, 3> eAT = expGyro(gyro.X, gyro.Y, gyro.Z, t); gyroOrientation = gyroOrientation * eAT; - NTypes::AccelerometerData zInIMUSpace; - RotateRocketVectorToIMUVector({ 0, 0, 1 }, zInIMUSpace); - // initial and startAxis are dependent on orientation_quat of imu - - Matrix<3, 1> initial{ { zInIMUSpace.X, zInIMUSpace.Y, zInIMUSpace.Z } }; - Matrix<3, 1> now = gyroOrientation * initial; - NTypes::AccelerometerData rotatedUsInRocketSpace{ 0, 0, 1 }; - RotateIMUVectorToRocketVector({ now.Get(0, 0), now.Get(1, 0), now.Get(2, 0) }, rotatedUsInRocketSpace); - - // initial = rotate([0 0 1] by quaternion) - // now = rotate initial by gyroOrientation - - auto dot = [](const Matrix<3, 1>& a, const Matrix<3, 1>& b) { - return a.Get(0, 0) * b.Get(0, 0) + a.Get(1, 0) * b.Get(1, 0) + a.Get(2, 0) * b.Get(2, 0); - }; - - float normOfNow = std::sqrt(initial.Get(0,0)*initial.Get(0,0) + initial.Get(1,0)*initial.Get(1,0) + initial.Get(2,0)*initial.Get(2,0)); - Matrix<3, 1> normedNow = initial * (1.F / normOfNow); - float offStart = acos(dot(initial, normedNow)); - bool outOfBounds = offStart > deg2rad(30); + const Matrix<3, 1>& initial = InitialRocketZAxisInIMUSpace(); + const Matrix<3, 1> now = gyroOrientation * initial; + const float nowNorm = std::sqrt(Dot3(now, now)); + const float cosOffStart = (nowNorm == 0.0F) ? 1.0F : ClampUnit(Dot3(initial, now) / nowNorm); + const bool outOfBounds = cosOffStart < kCosMaxTilt; everWentOutOfBounds |= outOfBounds; } From e46f8dc628a8ca39e461fec59cb20ef21cf44075 Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Thu, 9 Apr 2026 16:18:11 -0400 Subject: [PATCH 05/10] Don't divide by zero in the airbrake LUT --- app/other/airbraker/src/lut.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/app/other/airbraker/src/lut.cpp b/app/other/airbraker/src/lut.cpp index 6af2eacdf..30d141995 100644 --- a/app/other/airbraker/src/lut.cpp +++ b/app/other/airbraker/src/lut.cpp @@ -68,7 +68,11 @@ float CalcActuatorEffort(float altitude, float velocity) { float z_hat_min = 0; float z_hat_max = 0; boundsLUT(altitude, &z_hat_min, &z_hat_max); - float q = (velocity - z_hat_min) / (z_hat_max - z_hat_min); + const float span = z_hat_max - z_hat_min; + if (span <= 0.0F) { + return 0.0F; + } + float q = (velocity - z_hat_min) / span; if (q > 1) { return 1; } else if (q < 0) { @@ -77,4 +81,4 @@ float CalcActuatorEffort(float altitude, float velocity) { return q; } -} // namespace NModel \ No newline at end of file +} // namespace NModel From 09859329f24c68e9a26737bf952ab5ed20994f60 Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Mon, 13 Apr 2026 12:15:54 -0400 Subject: [PATCH 06/10] Formatting --- app/other/airbraker/src/main.cpp | 25 +- app/other/airbraker/src/n_model.cpp | 364 +++++++++++------- app/other/airbraker/src/n_preboost.cpp | 4 +- app/other/airbraker/src/n_sensing.cpp | 3 +- app/other/airbraker/src/n_storage.cpp | 4 +- app/other/airbraker/src/shell.cpp | 4 +- .../tools/benchmark_airbrake_math.cpp | 349 ++++++++++------- 7 files changed, 443 insertions(+), 310 deletions(-) diff --git a/app/other/airbraker/src/main.cpp b/app/other/airbraker/src/main.cpp index 00119c394..af4a5a8ef 100644 --- a/app/other/airbraker/src/main.cpp +++ b/app/other/airbraker/src/main.cpp @@ -7,12 +7,11 @@ #include "n_storage.hpp" #include "servo.hpp" +#include #include #include #include #include -#include - #include LOG_MODULE_REGISTER(main, CONFIG_APP_AIRBRAKE_LOG_LEVEL); @@ -34,12 +33,8 @@ uint32_t packet_timestamp() { return 0; \ } -NTypes::GyroscopeData unbiasGyro(const NTypes::GyroscopeData &data, const NTypes::GyroscopeData &bias){ - return { - .X = data.X - bias.X, - .Y = data.Y - bias.Y, - .Z = data.Z - bias.Z - }; +NTypes::GyroscopeData unbiasGyro(const NTypes::GyroscopeData &data, const NTypes::GyroscopeData &bias) { + return {.X = data.X - bias.X, .Y = data.Y - bias.Y, .Z = data.Z - bias.Z}; } int main() { @@ -128,7 +123,6 @@ int main() { float altMeters = NModel::AltitudeMetersFromPressureKPa(packet.pressureRaw) - groundLevelASLMeters; float vertical = GetUpAxis(packet.accelRaw); - NTypes::GyroscopeData unbiasedGyro = unbiasGyro(packet.gyro, bias); NModel::FeedGyro(packet.timestamp, unbiasedGyro); NModel::FillPacketWithOrientationMatrix(packet.orientationMatrix); @@ -148,8 +142,6 @@ int main() { NStorage::WriteFlightPacket(i, &packet); - - // Write preboost if needed if (preboostWriteHead < NUM_STORED_PREBOOST_PACKETS) { NStorage::WritePreboostPacket(preboostWriteHead, NPreBoost::GetPreBoostPacketPtr(preboostWriteHead)); @@ -161,7 +153,7 @@ int main() { CancelFlight(); // happy beep for reco team - while(true){ + while (true) { NBuzzer::SetBuzzer(true); k_msleep(200); NBuzzer::SetBuzzer(false); @@ -182,13 +174,12 @@ void CancelFlight() { } bool IsFlightCancelled() { return atomic_get(&flightCancelled) == 1; } - -float GetUpAxis(const NTypes::AccelerometerData &xyz){ - NTypes::AccelerometerData out{0,0,0}; +float GetUpAxis(const NTypes::AccelerometerData &xyz) { + NTypes::AccelerometerData out{0, 0, 0}; RotateIMUVectorToRocketVector(xyz, out); return out.Z; } -void RotateIMUVectorToRocketVector(const NTypes::AccelerometerData &xyz, NTypes::AccelerometerData &out){ +void RotateIMUVectorToRocketVector(const NTypes::AccelerometerData &xyz, NTypes::AccelerometerData &out) { zsl_quat p{.r = 0, .i = xyz.X, .j = xyz.Y, .k = xyz.Z}; // q p q* @@ -202,7 +193,7 @@ void RotateIMUVectorToRocketVector(const NTypes::AccelerometerData &xyz, NTypes: out.Z = output.k; } -void RotateRocketVectorToIMUVector(const NTypes::AccelerometerData &xyz, NTypes::AccelerometerData &out){ +void RotateRocketVectorToIMUVector(const NTypes::AccelerometerData &xyz, NTypes::AccelerometerData &out) { zsl_quat p{.r = 0, .i = xyz.X, .j = xyz.Y, .k = xyz.Z}; // q p q* diff --git a/app/other/airbraker/src/n_model.cpp b/app/other/airbraker/src/n_model.cpp index 91c2b79f4..bb413630b 100644 --- a/app/other/airbraker/src/n_model.cpp +++ b/app/other/airbraker/src/n_model.cpp @@ -2,197 +2,265 @@ #include "math/matrix.hpp" #include "n_autocoder_types.h" -#include "quantile_lut_data.h" #include "n_preboost.hpp" +#include "quantile_lut_data.h" #include -namespace NModel -{ -namespace -{ +namespace NModel { +namespace { constexpr float kCosMaxTilt = 0.8660254037844386F; +constexpr float kDefaultGyroDeltaS = 0.01F; +constexpr float kMaxGyroDeltaS = 0.05F; + +struct Vec3 { + float x; + float y; + float z; +}; + +struct Quaternion { + float w; + float x; + float y; + float z; +}; + +float ClampUnit(float v) { + if (v > 1.0F) { + return 1.0F; + } + if (v < -1.0F) { + return -1.0F; + } + return v; +} -float ClampUnit(float v) -{ - if (v > 1.0F) - { - return 1.0F; - } - if (v < -1.0F) - { - return -1.0F; - } - return v; +float Dot3(const Vec3& a, const Vec3& b) { return (a.x * b.x) + (a.y * b.y) + (a.z * b.z); } + +Vec3 Cross3(const Vec3& a, const Vec3& b) { + return { + .x = (a.y * b.z) - (a.z * b.y), + .y = (a.z * b.x) - (a.x * b.z), + .z = (a.x * b.y) - (a.y * b.x), + }; } -const Matrix<3, 1>& InitialRocketZAxisInIMUSpace() -{ - static const Matrix<3, 1> initial = [] { - NTypes::AccelerometerData zInIMUSpace{}; - RotateRocketVectorToIMUVector({ 0, 0, 1 }, zInIMUSpace); - return Matrix<3, 1>{ { zInIMUSpace.X, zInIMUSpace.Y, zInIMUSpace.Z } }; - }(); - return initial; +Quaternion QuaternionMultiply(const Quaternion& lhs, const Quaternion& rhs) { + return { + .w = (lhs.w * rhs.w) - (lhs.x * rhs.x) - (lhs.y * rhs.y) - (lhs.z * rhs.z), + .x = (lhs.w * rhs.x) + (lhs.x * rhs.w) + (lhs.y * rhs.z) - (lhs.z * rhs.y), + .y = (lhs.w * rhs.y) - (lhs.x * rhs.z) + (lhs.y * rhs.w) + (lhs.z * rhs.x), + .z = (lhs.w * rhs.z) + (lhs.x * rhs.y) - (lhs.y * rhs.x) + (lhs.z * rhs.w), + }; } -float Dot3(const Matrix<3, 1>& a, const Matrix<3, 1>& b) -{ - return (a.Get(0, 0) * b.Get(0, 0)) + (a.Get(1, 0) * b.Get(1, 0)) + (a.Get(2, 0) * b.Get(2, 0)); +Quaternion NormalizeQuaternion(const Quaternion& q) { + const float normSq = (q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z); + if (normSq == 0.0F) { + return {1.0F, 0.0F, 0.0F, 0.0F}; + } + + const float invNorm = 1.0F / std::sqrt(normSq); + return { + .w = q.w * invNorm, + .x = q.x * invNorm, + .y = q.y * invNorm, + .z = q.z * invNorm, + }; } -} // namespace -const char* GetMatlabLUTName() -{ - return LUT_NAME; +Quaternion QuaternionDeltaFromGyro(const NTypes::GyroscopeData& gyro, float deltaSeconds) { + const float rx = gyro.X * deltaSeconds; + const float ry = gyro.Y * deltaSeconds; + const float rz = gyro.Z * deltaSeconds; + const float rotationMagnitudeSq = (rx * rx) + (ry * ry) + (rz * rz); + const float rotationMagnitude = std::sqrt(rotationMagnitudeSq); + + if (rotationMagnitude == 0.0F) { + return {1.0F, 0.0F, 0.0F, 0.0F}; + } + + const float halfAngle = 0.5F * rotationMagnitude; + const float vectorScale = std::sin(halfAngle) / rotationMagnitude; + return { + .w = std::cos(halfAngle), + .x = rx * vectorScale, + .y = ry * vectorScale, + .z = rz * vectorScale, + }; } -const char* GetMatlabLUTDate() -{ - return LUT_CREATION_DATE; + +Vec3 RotateVectorByQuaternion(const Quaternion& q, const Vec3& v) { + const Vec3 u{q.x, q.y, q.z}; + const Vec3 uv = Cross3(u, v); + const Vec3 uuv = Cross3(u, uv); + return { + .x = v.x + (2.0F * ((q.w * uv.x) + uuv.x)), + .y = v.y + (2.0F * ((q.w * uv.y) + uuv.y)), + .z = v.z + (2.0F * ((q.w * uv.z) + uuv.z)), + }; } +const Vec3& InitialRocketZAxisInIMUSpace() { + static const Vec3 initial = [] { + NTypes::AccelerometerData zInIMUSpace{}; + RotateRocketVectorToIMUVector({0, 0, 1}, zInIMUSpace); + return Vec3{zInIMUSpace.X, zInIMUSpace.Y, zInIMUSpace.Z}; + }(); + return initial; +} +} // namespace + +const char* GetMatlabLUTName() { return LUT_NAME; } +const char* GetMatlabLUTDate() { return LUT_CREATION_DATE; } + using StateTransitionT = Matrix<4, 4>; -const StateTransitionT state_transition_matrix{ { // aka F - KALMAN_STATE_TRANSITION_INITIALIZER } }; +const StateTransitionT state_transition_matrix{{// aka F + KALMAN_STATE_TRANSITION_INITIALIZER}}; -const Matrix<2, 4> kalman_output_matrix{ { // aka H - KALMAN_OUTPUT_INITIALIZER } }; -const Matrix<4, 2> kalman_gain{ { KALMAN_GAIN_INITIALIZER } }; +const Matrix<2, 4> kalman_output_matrix{{// aka H + KALMAN_OUTPUT_INITIALIZER}}; +const Matrix<4, 2> kalman_gain{{KALMAN_GAIN_INITIALIZER}}; -static Matrix<4, 1> kalman_state({ KALMAN_INITIAL_STATE_INITIALIZER }); +static Matrix<4, 1> kalman_state({KALMAN_INITIAL_STATE_INITIALIZER}); -static Matrix<2, 1> lastInnovation{ { 0, 0 } }; -static Matrix<3, 3> gyroOrientation = Matrix<3, 3>::Identity(); +static Matrix<2, 1> lastInnovation{{0, 0}}; +static Quaternion gyroOrientation{1.0F, 0.0F, 0.0F, 0.0F}; static bool everWentOutOfBounds = false; Matrix<4, 1> kalmanPredictAndUpdate(const Matrix<4, 1>& state, const float altitudeMeters, - const float verticalAccelerationMS2) -{ - Matrix<2, 1> sensorIn = Matrix<2, 1>::Column({ altitudeMeters, verticalAccelerationMS2 }); + const float verticalAccelerationMS2) { + Matrix<2, 1> sensorIn = Matrix<2, 1>::Column({altitudeMeters, verticalAccelerationMS2}); - // change via physics model - Matrix<4, 1> fx = state_transition_matrix * state; + // change via physics model + Matrix<4, 1> fx = state_transition_matrix * state; - // change via sensors - Matrix<2, 1> innovation = sensorIn - (kalman_output_matrix * fx); - lastInnovation = innovation; + // change via sensors + Matrix<2, 1> innovation = sensorIn - (kalman_output_matrix * fx); + lastInnovation = innovation; - Matrix<4, 1> correction = kalman_gain * innovation; + Matrix<4, 1> correction = kalman_gain * innovation; - return fx + correction; + return fx + correction; } -void FeedKalman(float altitudeMeters, float verticalAccelerationMS2) -{ - kalman_state = kalmanPredictAndUpdate(kalman_state, altitudeMeters, verticalAccelerationMS2); +void FeedKalman(float altitudeMeters, float verticalAccelerationMS2) { + kalman_state = kalmanPredictAndUpdate(kalman_state, altitudeMeters, verticalAccelerationMS2); } -KalmanState LastKalmanState() -{ - return { - .estAltitude = kalman_state.Get(0, 0), - .estVelocity = kalman_state.Get(1, 0), - .estAcceleration = kalman_state.Get(2, 0), - .estBias = kalman_state.Get(3, 0), - }; +KalmanState LastKalmanState() { + return { + .estAltitude = kalman_state.Get(0, 0), + .estVelocity = kalman_state.Get(1, 0), + .estAcceleration = kalman_state.Get(2, 0), + .estBias = kalman_state.Get(3, 0), + }; } #define CUSTOM_ATMOSPHERE 1 #ifdef CUSTOM_ATMOSPHERE -float AltitudeMetersFromPressureKPa(float kPa) -{ - const float x = kPa * 1000.0F; - float sum = ATMOSPHERE[AUTOGEN_ATMOSPHERE_NUM_COEFFECIENTS - 1]; - for (size_t i = AUTOGEN_ATMOSPHERE_NUM_COEFFECIENTS - 1; i-- > 0;) - { - sum = (sum * x) + ATMOSPHERE[i]; - } - return sum; +float AltitudeMetersFromPressureKPa(float kPa) { + const float x = kPa * 1000.0F; + float sum = ATMOSPHERE[AUTOGEN_ATMOSPHERE_NUM_COEFFECIENTS - 1]; + for (size_t i = AUTOGEN_ATMOSPHERE_NUM_COEFFECIENTS - 1; i-- > 0;) { + sum = (sum * x) + ATMOSPHERE[i]; + } + return sum; } #else -float AltitudeMetersFromPressureKPa(float pressure_kpa) -{ - float pressure = pressure_kpa * 10; - float altitude = (1 - powf(pressure / 1013.25F, 0.190284F)) * (float)(145366.45 * 0.3048); - return altitude; +float AltitudeMetersFromPressureKPa(float pressure_kpa) { + float pressure = pressure_kpa * 10; + float altitude = (1 - powf(pressure / 1013.25F, 0.190284F)) * (float) (145366.45 * 0.3048); + return altitude; } #endif -Matrix<3, 3> expGyro(float w_1, float w_2, float w_3, float t) -{ - const float wx = w_1 * t; - const float wy = w_2 * t; - const float wz = w_3 * t; +Matrix<3, 3> expGyro(float w_1, float w_2, float w_3, float t) { + const float wx = w_1 * t; + const float wy = w_2 * t; + const float wz = w_3 * t; - const float wx2 = wx * wx; - const float wy2 = wy * wy; - const float wz2 = wz * wz; - const float wxwy = wx * wy; - const float wxwz = wx * wz; - const float wywz = wy * wz; + const float wx2 = wx * wx; + const float wy2 = wy * wy; + const float wz2 = wz * wz; + const float wxwy = wx * wy; + const float wxwz = wx * wz; + const float wywz = wy * wz; - const float thetaSq = wx2 + wy2 + wz2; - const float theta = std::sqrt(thetaSq); - const float s = (theta == 0.0F) ? 1.0F : (std::sin(theta) / theta); - const float c = (theta == 0.0F) ? 0.0F : ((1.0F - std::cos(theta)) / thetaSq); + const float thetaSq = wx2 + wy2 + wz2; + const float theta = std::sqrt(thetaSq); + const float s = (theta == 0.0F) ? 1.0F : (std::sin(theta) / theta); + const float c = (theta == 0.0F) ? 0.0F : ((1.0F - std::cos(theta)) / thetaSq); - return Matrix<3, 3>{ { - 1.0F - c * (wy2 + wz2), c * wxwy - s * wz, c * wxwz + s * wy, - c * wxwy + s * wz, 1.0F - c * (wx2 + wz2), c * wywz - s * wx, - c * wxwz - s * wy, c * wywz + s * wx, 1.0F - c * (wx2 + wy2), - } }; + return Matrix<3, 3>{{ + 1.0F - c * (wy2 + wz2), + c * wxwy - s * wz, + c * wxwz + s * wy, + c * wxwy + s * wz, + 1.0F - c * (wx2 + wz2), + c * wywz - s * wx, + c * wxwz - s * wy, + c * wywz + s * wx, + 1.0F - c * (wx2 + wy2), + }}; } -float deg2rad(float d) -{ - return d / 180.0F * 3.14159F; +float deg2rad(float d) { return d / 180.0F * 3.14159F; } +float rad2deg(float d) { return d * 180.0F / 3.14159F; } + +static uint32_t lastGyroIntegrationMsSinceBoot = 0; +void FeedGyro(uint32_t msSinceBoot, const NTypes::GyroscopeData& gyro) { + float t = kDefaultGyroDeltaS; + if (lastGyroIntegrationMsSinceBoot != 0) { + const int32_t deltaMs = (int32_t) (msSinceBoot - lastGyroIntegrationMsSinceBoot); + if (deltaMs > 0) { + t = static_cast(deltaMs) / 1000.F; + if (t > kMaxGyroDeltaS) { + t = kMaxGyroDeltaS; + } + } + } + lastGyroIntegrationMsSinceBoot = msSinceBoot; + const Quaternion delta = QuaternionDeltaFromGyro(gyro, t); + gyroOrientation = NormalizeQuaternion(QuaternionMultiply(gyroOrientation, delta)); + + const Vec3& initial = InitialRocketZAxisInIMUSpace(); + const Vec3 now = RotateVectorByQuaternion(gyroOrientation, initial); + const float cosOffStart = ClampUnit(Dot3(initial, now)); + const bool outOfBounds = cosOffStart < kCosMaxTilt; + everWentOutOfBounds |= outOfBounds; } -float rad2deg(float d) -{ - return d * 180.0F / 3.14159F; + +int GetOrientation() { return 0; } +bool EverWentOutOfBounds() { return everWentOutOfBounds; } + +void FillPacketWithOrientationMatrix(float* arr) { + const float ww = gyroOrientation.w * gyroOrientation.w; + const float xx = gyroOrientation.x * gyroOrientation.x; + const float yy = gyroOrientation.y * gyroOrientation.y; + const float zz = gyroOrientation.z * gyroOrientation.z; + const float wx = gyroOrientation.w * gyroOrientation.x; + const float wy = gyroOrientation.w * gyroOrientation.y; + const float wz = gyroOrientation.w * gyroOrientation.z; + const float xy = gyroOrientation.x * gyroOrientation.y; + const float xz = gyroOrientation.x * gyroOrientation.z; + const float yz = gyroOrientation.y * gyroOrientation.z; + + arr[0] = ww + xx - yy - zz; + arr[1] = 2.0F * (xy - wz); + arr[2] = 2.0F * (xz + wy); + arr[3] = 2.0F * (xy + wz); + arr[4] = ww - xx + yy - zz; + arr[5] = 2.0F * (yz - wx); + arr[6] = 2.0F * (xz - wy); + arr[7] = 2.0F * (yz + wx); + arr[8] = ww - xx - yy + zz; } -static uint32_t lastGyroIntegrationMsSinceBoot = 0; -void FeedGyro(uint32_t msSinceBoot, const NTypes::GyroscopeData& gyro) -{ - float t = 0.01; - if (lastGyroIntegrationMsSinceBoot != 0) - { - const int32_t deltaMs = (int32_t)(msSinceBoot - lastGyroIntegrationMsSinceBoot); - t = static_cast(deltaMs) / 1000.F; - } - lastGyroIntegrationMsSinceBoot = msSinceBoot; - Matrix<3, 3> eAT = expGyro(gyro.X, gyro.Y, gyro.Z, t); - gyroOrientation = gyroOrientation * eAT; - const Matrix<3, 1>& initial = InitialRocketZAxisInIMUSpace(); - const Matrix<3, 1> now = gyroOrientation * initial; - const float nowNorm = std::sqrt(Dot3(now, now)); - const float cosOffStart = (nowNorm == 0.0F) ? 1.0F : ClampUnit(Dot3(initial, now) / nowNorm); - const bool outOfBounds = cosOffStart < kCosMaxTilt; - everWentOutOfBounds |= outOfBounds; -} - -int GetOrientation() -{ - return 0; -} -bool EverWentOutOfBounds() -{ - return everWentOutOfBounds; -} - -void FillPacketWithOrientationMatrix(float* arr) -{ - for (size_t i = 0; i < 9; i++) - { - arr[i] = gyroOrientation.Get(i / 3, i % 3); - } -} - -void FillPacketWithKalmanInformation(float* inno, KalmanState &state) -{ - inno[0] = lastInnovation.Get(0, 0); - inno[1] = lastInnovation.Get(1, 0); - state = LastKalmanState(); -} - -} // namespace NModel +void FillPacketWithKalmanInformation(float* inno, KalmanState& state) { + inno[0] = lastInnovation.Get(0, 0); + inno[1] = lastInnovation.Get(1, 0); + state = LastKalmanState(); +} + +} // namespace NModel diff --git a/app/other/airbraker/src/n_preboost.cpp b/app/other/airbraker/src/n_preboost.cpp index d26f892cd..96a02209f 100644 --- a/app/other/airbraker/src/n_preboost.cpp +++ b/app/other/airbraker/src/n_preboost.cpp @@ -44,14 +44,14 @@ void SubmitPreBoostPacket(const Packet &packet) { preboostPackets.AddSample(packet); Packet *newestSampleForGyroBias = &preboostPackets.OldestSample(); gyroBiasAverager.Feed({newestSampleForGyroBias->gyro}); - + // Grab ground level altitude from most recent if we're before the circular buffer is initialized if (preboostPackets.OldestSample().pressureRaw == 0) { groundLevelPressure = packet.pressureRaw; } else { groundLevelPressure = preboostPackets.OldestSample().pressureRaw; } - groundLevelASLMeters = NModel::AltitudeMetersFromPressureKPa(groundLevelPressure); + groundLevelASLMeters = NModel::AltitudeMetersFromPressureKPa(groundLevelPressure); } NTypes::GyroscopeData GetGyroBias() { return gyroBiasAverager.Avg().internal; } diff --git a/app/other/airbraker/src/n_sensing.cpp b/app/other/airbraker/src/n_sensing.cpp index 5a59984d6..47df19603 100644 --- a/app/other/airbraker/src/n_sensing.cpp +++ b/app/other/airbraker/src/n_sensing.cpp @@ -33,7 +33,8 @@ int InitSensors() { return 0; } -int MeasureSensors(float &tempC, float &pressureKPa, NTypes::AccelerometerData &accelMs2, NTypes::GyroscopeData &gyroDps) { +int MeasureSensors(float &tempC, float &pressureKPa, NTypes::AccelerometerData &accelMs2, + NTypes::GyroscopeData &gyroDps) { // todo, can make this less noisy by kicking off fetch, then doing other stuff, then reading int bret = sensor_sample_fetch(barom_dev); if (bret < 0) { diff --git a/app/other/airbraker/src/n_storage.cpp b/app/other/airbraker/src/n_storage.cpp index 428351f98..581adfd2a 100644 --- a/app/other/airbraker/src/n_storage.cpp +++ b/app/other/airbraker/src/n_storage.cpp @@ -236,8 +236,8 @@ uint32_t GetDataPartitionAddress() { return FLIGHT_PARTITION_OFFSET; } uint32_t GetParamPartitionSize() { return PARAM_PARTITION_SIZE; } uint32_t GetDataPartitionSize() { return FLIGHT_PARTITION_SIZE; } -int ReadDataBlock(uint32_t dataAddr, uint32_t size, uint8_t*data){ - if (dataAddr > FLIGHT_PARTITION_SIZE){ +int ReadDataBlock(uint32_t dataAddr, uint32_t size, uint8_t *data) { + if (dataAddr > FLIGHT_PARTITION_SIZE) { // out of bounds return -1; } diff --git a/app/other/airbraker/src/shell.cpp b/app/other/airbraker/src/shell.cpp index f6cf30a1c..68ffa6f4c 100644 --- a/app/other/airbraker/src/shell.cpp +++ b/app/other/airbraker/src/shell.cpp @@ -1,9 +1,9 @@ #include "common.hpp" +#include "n_boost.hpp" #include "n_buzzer.hpp" #include "n_model.hpp" #include "n_sensing.hpp" #include "n_storage.hpp" -#include "n_boost.hpp" #include "servo.hpp" #include @@ -409,7 +409,7 @@ static int cmd_fakeboost(const struct shell *shell, size_t argc, char **argv) { } long seconds = 0; bool parsed = parse_long(argv[1], &seconds); - if (!parsed){ + if (!parsed) { shell_error(shell, "Invalid number of seconds to wait for"); } diff --git a/app/other/airbraker/tools/benchmark_airbrake_math.cpp b/app/other/airbraker/tools/benchmark_airbrake_math.cpp index 241422271..8d1de2e1a 100644 --- a/app/other/airbraker/tools/benchmark_airbrake_math.cpp +++ b/app/other/airbraker/tools/benchmark_airbrake_math.cpp @@ -13,6 +13,14 @@ namespace { using Matrix3 = ManualMatrix<3, 3, float>; using Vector3 = ManualMatrix<3, 1, float>; +using ArrayVec3 = std::array; + +struct Quaternion { + float w; + float x; + float y; + float z; +}; constexpr std::array kAtmosphere = { 1.2345e2F, -1.876e-2F, 3.4e-6F, -5.1e-10F, 7.2e-14F, -3.3e-18F, 6.8e-23F, @@ -25,43 +33,43 @@ constexpr std::array, 3> kImuToRocket = {{ }}; template -inline void DoNotOptimize(T const& value) -{ +inline void DoNotOptimize(T const& value) { asm volatile("" : : "g"(value) : "memory"); } -float ClampUnit(float v) -{ - return std::max(-1.0F, std::min(1.0F, v)); -} +float ClampUnit(float v) { return std::max(-1.0F, std::min(1.0F, v)); } -float Dot3(const Vector3& a, const Vector3& b) -{ +float Dot3(const Vector3& a, const Vector3& b) { return (a.Get(0, 0) * b.Get(0, 0)) + (a.Get(1, 0) * b.Get(1, 0)) + (a.Get(2, 0) * b.Get(2, 0)); } -std::array RotateImuToRocket(const std::array& v) -{ +float Dot3(const ArrayVec3& a, const ArrayVec3& b) { return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]); } + +ArrayVec3 Cross3(const ArrayVec3& a, const ArrayVec3& b) { + return { + (a[1] * b[2]) - (a[2] * b[1]), + (a[2] * b[0]) - (a[0] * b[2]), + (a[0] * b[1]) - (a[1] * b[0]), + }; +} + +std::array RotateImuToRocket(const std::array& v) { std::array out{}; - for (int r = 0; r < 3; ++r) - { + for (int r = 0; r < 3; ++r) { out[r] = (kImuToRocket[r][0] * v[0]) + (kImuToRocket[r][1] * v[1]) + (kImuToRocket[r][2] * v[2]); } return out; } -std::array RotateRocketToImu(const std::array& v) -{ +std::array RotateRocketToImu(const std::array& v) { std::array out{}; - for (int r = 0; r < 3; ++r) - { + for (int r = 0; r < 3; ++r) { out[r] = (kImuToRocket[0][r] * v[0]) + (kImuToRocket[1][r] * v[1]) + (kImuToRocket[2][r] * v[2]); } return out; } -Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) -{ +Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) { w1 *= t; w2 *= t; w3 *= t; @@ -74,14 +82,26 @@ Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) const float w2w3 = w2 * w3; Matrix3 A{{ - 0, -w3, w2, - w3, 0, -w1, - -w2, w1, 0, + 0, + -w3, + w2, + w3, + 0, + -w1, + -w2, + w1, + 0, }}; Matrix3 A2{{ - -(w2_2 + w3_2), w1w2, w1w3, - w1w2, -(w1_2 + w3_2), w2w3, - w1w3, w2w3, -(w1_2 + w2_2), + -(w2_2 + w3_2), + w1w2, + w1w3, + w1w2, + -(w1_2 + w3_2), + w2w3, + w1w3, + w2w3, + -(w1_2 + w2_2), }}; const float normSq = w1_2 + w2_2 + w3_2; @@ -92,8 +112,7 @@ Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) return Matrix3::Identity() + (A * s) + (A2 * c); } -Matrix3 ExpGyroNew(float w1, float w2, float w3, float t) -{ +Matrix3 ExpGyroNew(float w1, float w2, float w3, float t) { const float wx = w1 * t; const float wy = w2 * t; const float wz = w3 * t; @@ -111,43 +130,95 @@ Matrix3 ExpGyroNew(float w1, float w2, float w3, float t) const float c = (theta == 0.0F) ? 0.0F : ((1.0F - std::cos(theta)) / thetaSq); return Matrix3{{ - 1.0F - c * (wy2 + wz2), c * wxwy - s * wz, c * wxwz + s * wy, - c * wxwy + s * wz, 1.0F - c * (wx2 + wz2), c * wywz - s * wx, - c * wxwz - s * wy, c * wywz + s * wx, 1.0F - c * (wx2 + wy2), + 1.0F - c * (wy2 + wz2), + c * wxwy - s * wz, + c * wxwz + s * wy, + c * wxwy + s * wz, + 1.0F - c * (wx2 + wz2), + c * wywz - s * wx, + c * wxwz - s * wy, + c * wywz + s * wx, + 1.0F - c * (wx2 + wy2), }}; } -float AltitudeOriginal(float kPa) -{ +float AltitudeOriginal(float kPa) { const float x = kPa * 1000.0F; float sum = 0.0F; float xn = x; - for (std::size_t i = 1; i < kAtmosphere.size(); ++i) - { + for (std::size_t i = 1; i < kAtmosphere.size(); ++i) { sum += kAtmosphere[i] * xn; xn *= x; } return sum + kAtmosphere[0]; } -float AltitudeNew(float kPa) -{ +float AltitudeNew(float kPa) { const float x = kPa * 1000.0F; float sum = kAtmosphere[kAtmosphere.size() - 1]; - for (std::size_t i = kAtmosphere.size() - 1; i-- > 0;) - { + for (std::size_t i = kAtmosphere.size() - 1; i-- > 0;) { sum = (sum * x) + kAtmosphere[i]; } return sum; } +Quaternion QuaternionMultiply(const Quaternion& lhs, const Quaternion& rhs) { + return { + (lhs.w * rhs.w) - (lhs.x * rhs.x) - (lhs.y * rhs.y) - (lhs.z * rhs.z), + (lhs.w * rhs.x) + (lhs.x * rhs.w) + (lhs.y * rhs.z) - (lhs.z * rhs.y), + (lhs.w * rhs.y) - (lhs.x * rhs.z) + (lhs.y * rhs.w) + (lhs.z * rhs.x), + (lhs.w * rhs.z) + (lhs.x * rhs.y) - (lhs.y * rhs.x) + (lhs.z * rhs.w), + }; +} + +Quaternion NormalizeQuaternion(const Quaternion& q) { + const float normSq = (q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z); + if (normSq == 0.0F) { + return {1.0F, 0.0F, 0.0F, 0.0F}; + } + + const float invNorm = 1.0F / std::sqrt(normSq); + return {q.w * invNorm, q.x * invNorm, q.y * invNorm, q.z * invNorm}; +} + +Quaternion QuaternionDeltaFromGyro(float gx, float gy, float gz, float dt) { + const float rx = gx * dt; + const float ry = gy * dt; + const float rz = gz * dt; + const float rotationMagnitudeSq = (rx * rx) + (ry * ry) + (rz * rz); + const float rotationMagnitude = std::sqrt(rotationMagnitudeSq); + + if (rotationMagnitude == 0.0F) { + return {1.0F, 0.0F, 0.0F, 0.0F}; + } + + const float halfAngle = 0.5F * rotationMagnitude; + const float vectorScale = std::sin(halfAngle) / rotationMagnitude; + return {std::cos(halfAngle), rx * vectorScale, ry * vectorScale, rz * vectorScale}; +} + +ArrayVec3 RotateVectorByQuaternion(const Quaternion& q, const ArrayVec3& v) { + const ArrayVec3 u{q.x, q.y, q.z}; + const ArrayVec3 uv = Cross3(u, v); + const ArrayVec3 uuv = Cross3(u, uv); + return { + v[0] + (2.0F * ((q.w * uv[0]) + uuv[0])), + v[1] + (2.0F * ((q.w * uv[1]) + uuv[1])), + v[2] + (2.0F * ((q.w * uv[2]) + uuv[2])), + }; +} + struct GyroStepResult { Matrix3 orientation; bool outOfBounds; }; -GyroStepResult FeedGyroOriginal(Matrix3 orientation, float gx, float gy, float gz, float dt) -{ +struct QuaternionGyroStepResult { + Quaternion orientation; + bool outOfBounds; +}; + +GyroStepResult FeedGyroOriginal(Matrix3 orientation, float gx, float gy, float gz, float dt) { const Matrix3 eAT = ExpGyroOriginal(gx, gy, gz, dt); orientation = orientation * eAT; @@ -156,33 +227,27 @@ GyroStepResult FeedGyroOriginal(Matrix3 orientation, float gx, float gy, float g const Vector3 now = orientation * initial; const auto rotated = RotateImuToRocket({now.Get(0, 0), now.Get(1, 0), now.Get(2, 0)}); - (void)rotated; + (void) rotated; - const float initialNorm = std::sqrt((initial.Get(0, 0) * initial.Get(0, 0)) + - (initial.Get(1, 0) * initial.Get(1, 0)) + - (initial.Get(2, 0) * initial.Get(2, 0))); + const float initialNorm = + std::sqrt((initial.Get(0, 0) * initial.Get(0, 0)) + (initial.Get(1, 0) * initial.Get(1, 0)) + + (initial.Get(2, 0) * initial.Get(2, 0))); const Vector3 normedNow = initial * (1.0F / initialNorm); - const float dot = (initial.Get(0, 0) * normedNow.Get(0, 0)) + - (initial.Get(1, 0) * normedNow.Get(1, 0)) + + const float dot = (initial.Get(0, 0) * normedNow.Get(0, 0)) + (initial.Get(1, 0) * normedNow.Get(1, 0)) + (initial.Get(2, 0) * normedNow.Get(2, 0)); const float offStart = std::acos(dot); return {orientation, offStart > (30.0F * 3.14159F / 180.0F)}; } -GyroStepResult FeedGyroNew(Matrix3 orientation, float gx, float gy, float gz, float dt) -{ +QuaternionGyroStepResult FeedGyroNew(Quaternion orientation, float gx, float gy, float gz, float dt) { constexpr float kCosMaxTilt = 0.8660254037844386F; - static const Vector3 initial = [] { - const auto zInImu = RotateRocketToImu({0.0F, 0.0F, 1.0F}); - return Vector3{{zInImu[0], zInImu[1], zInImu[2]}}; - }(); + static const ArrayVec3 initial = RotateRocketToImu({0.0F, 0.0F, 1.0F}); - const Matrix3 eAT = ExpGyroNew(gx, gy, gz, dt); - orientation = orientation * eAT; + const Quaternion delta = QuaternionDeltaFromGyro(gx, gy, gz, dt); + orientation = NormalizeQuaternion(QuaternionMultiply(orientation, delta)); - const Vector3 now = orientation * initial; - const float nowNorm = std::sqrt(Dot3(now, now)); - const float cosOffStart = (nowNorm == 0.0F) ? 1.0F : ClampUnit(Dot3(initial, now) / nowNorm); + const ArrayVec3 now = RotateVectorByQuaternion(orientation, initial); + const float cosOffStart = ClampUnit(Dot3(initial, now)); return {orientation, cosOffStart < kCosMaxTilt}; } @@ -192,12 +257,10 @@ struct Stats { }; template -Stats RunBenchmark(const char* name, Fn&& fn, std::size_t iterations, std::size_t rounds) -{ +Stats RunBenchmark(const char* name, Fn&& fn, std::size_t iterations, std::size_t rounds) { std::vector samples; samples.reserve(rounds); - for (std::size_t round = 0; round < rounds; ++round) - { + for (std::size_t round = 0; round < rounds; ++round) { const auto start = std::chrono::steady_clock::now(); fn(iterations); const auto end = std::chrono::steady_clock::now(); @@ -211,18 +274,16 @@ Stats RunBenchmark(const char* name, Fn&& fn, std::size_t iterations, std::size_ return {minNs, meanNs}; } -} // namespace +} // namespace -int main() -{ +int main() { constexpr std::size_t kIterations = 1'000'000; constexpr std::size_t kRounds = 8; volatile float sink = 0.0F; volatile bool sinkBool = false; - for (std::size_t i = 0; i < 10000; ++i) - { + for (std::size_t i = 0; i < 10000; ++i) { const float gx = 0.02F + static_cast(i % 11) * 0.004F; const float gy = -0.03F + static_cast(i % 7) * 0.005F; const float gz = 0.01F + static_cast(i % 13) * 0.003F; @@ -235,79 +296,91 @@ int main() std::puts("Airbraker math benchmark"); std::printf("iterations=%zu rounds=%zu\n\n", kIterations, kRounds); - const auto expOriginal = RunBenchmark("expGyro original", [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) - { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); - } - DoNotOptimize(sink); - }, kIterations, kRounds); - - const auto expNew = RunBenchmark("expGyro new", [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) - { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); - } - DoNotOptimize(sink); - }, kIterations, kRounds); - - const auto altOriginal = RunBenchmark("altitude original", [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) - { - sink += AltitudeOriginal(80.0F + static_cast(i % 1000) * 0.02F); - } - DoNotOptimize(sink); - }, kIterations, kRounds); - - const auto altNew = RunBenchmark("altitude new", [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) - { - sink += AltitudeNew(80.0F + static_cast(i % 1000) * 0.02F); - } - DoNotOptimize(sink); - }, kIterations, kRounds); - - const auto feedOriginal = RunBenchmark("feedGyro original", [&](std::size_t iterations) { - Matrix3 orientation = Matrix3::Identity(); - bool everOut = false; - for (std::size_t i = 0; i < iterations; ++i) - { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - const auto result = FeedGyroOriginal(orientation, gx, gy, gz, 0.01F); - orientation = result.orientation; - everOut |= result.outOfBounds; - } - sink += orientation.Get(0, 0); - sinkBool ^= everOut; - DoNotOptimize(sink); - DoNotOptimize(sinkBool); - }, kIterations, kRounds); - - const auto feedNew = RunBenchmark("feedGyro new", [&](std::size_t iterations) { - Matrix3 orientation = Matrix3::Identity(); - bool everOut = false; - for (std::size_t i = 0; i < iterations; ++i) - { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - const auto result = FeedGyroNew(orientation, gx, gy, gz, 0.01F); - orientation = result.orientation; - everOut |= result.outOfBounds; - } - sink += orientation.Get(0, 0); - sinkBool ^= everOut; - DoNotOptimize(sink); - DoNotOptimize(sinkBool); - }, kIterations, kRounds); + const auto expOriginal = RunBenchmark( + "expGyro original", + [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); + } + DoNotOptimize(sink); + }, + kIterations, kRounds); + + const auto expNew = RunBenchmark( + "expGyro new", + [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); + } + DoNotOptimize(sink); + }, + kIterations, kRounds); + + const auto altOriginal = RunBenchmark( + "altitude original", + [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) { + sink += AltitudeOriginal(80.0F + static_cast(i % 1000) * 0.02F); + } + DoNotOptimize(sink); + }, + kIterations, kRounds); + + const auto altNew = RunBenchmark( + "altitude new", + [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) { + sink += AltitudeNew(80.0F + static_cast(i % 1000) * 0.02F); + } + DoNotOptimize(sink); + }, + kIterations, kRounds); + + const auto feedOriginal = RunBenchmark( + "feedGyro original", + [&](std::size_t iterations) { + Matrix3 orientation = Matrix3::Identity(); + bool everOut = false; + for (std::size_t i = 0; i < iterations; ++i) { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + const auto result = FeedGyroOriginal(orientation, gx, gy, gz, 0.01F); + orientation = result.orientation; + everOut |= result.outOfBounds; + } + sink += orientation.Get(0, 0); + sinkBool ^= everOut; + DoNotOptimize(sink); + DoNotOptimize(sinkBool); + }, + kIterations, kRounds); + + const auto feedNew = RunBenchmark( + "feedGyro new", + [&](std::size_t iterations) { + Quaternion orientation{1.0F, 0.0F, 0.0F, 0.0F}; + bool everOut = false; + for (std::size_t i = 0; i < iterations; ++i) { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + const auto result = FeedGyroNew(orientation, gx, gy, gz, 0.01F); + orientation = result.orientation; + everOut |= result.outOfBounds; + } + sink += orientation.w; + sinkBool ^= everOut; + DoNotOptimize(sink); + DoNotOptimize(sinkBool); + }, + kIterations, kRounds); std::puts("\nSpeedups based on best run"); std::printf("expGyro: %.2fx\n", expOriginal.minNs / expNew.minNs); From 49211293d3c7a6445636ec427da3f432c29500a5 Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Mon, 13 Apr 2026 12:29:18 -0400 Subject: [PATCH 07/10] bring back some blursed variable names for richie --- app/other/airbraker/src/n_model.cpp | 48 +++++++++---------- .../tools/benchmark_airbrake_math.cpp | 48 +++++++++---------- 2 files changed, 48 insertions(+), 48 deletions(-) diff --git a/app/other/airbraker/src/n_model.cpp b/app/other/airbraker/src/n_model.cpp index bb413630b..d6b0f4626 100644 --- a/app/other/airbraker/src/n_model.cpp +++ b/app/other/airbraker/src/n_model.cpp @@ -176,32 +176,32 @@ float AltitudeMetersFromPressureKPa(float pressure_kpa) { #endif Matrix<3, 3> expGyro(float w_1, float w_2, float w_3, float t) { - const float wx = w_1 * t; - const float wy = w_2 * t; - const float wz = w_3 * t; - - const float wx2 = wx * wx; - const float wy2 = wy * wy; - const float wz2 = wz * wz; - const float wxwy = wx * wy; - const float wxwz = wx * wz; - const float wywz = wy * wz; - - const float thetaSq = wx2 + wy2 + wz2; - const float theta = std::sqrt(thetaSq); - const float s = (theta == 0.0F) ? 1.0F : (std::sin(theta) / theta); - const float c = (theta == 0.0F) ? 0.0F : ((1.0F - std::cos(theta)) / thetaSq); + const float w_1ᵗ = w_1 * t; + const float w_2ᵗ = w_2 * t; + const float w_3ᵗ = w_3 * t; + + const float w_1² = w_1ᵗ * w_1ᵗ; + const float w_2² = w_2ᵗ * w_2ᵗ; + const float w_3² = w_3ᵗ * w_3ᵗ; + const float w_1w_2 = w_1ᵗ * w_2ᵗ; + const float w_1w_3 = w_1ᵗ * w_3ᵗ; + const float w_2w_3 = w_2ᵗ * w_3ᵗ; + + const float θ² = w_1² + w_2² + w_3²; + const float θ = std::sqrt(θ²); + const float s = (θ == 0.0F) ? 1.0F : (std::sin(θ) / θ); + const float c = (θ == 0.0F) ? 0.0F : ((1.0F - std::cos(θ)) / θ²); return Matrix<3, 3>{{ - 1.0F - c * (wy2 + wz2), - c * wxwy - s * wz, - c * wxwz + s * wy, - c * wxwy + s * wz, - 1.0F - c * (wx2 + wz2), - c * wywz - s * wx, - c * wxwz - s * wy, - c * wywz + s * wx, - 1.0F - c * (wx2 + wy2), + 1.0F - c * (w_2² + w_3²), + c * w_1w_2 - s * w_3ᵗ, + c * w_1w_3 + s * w_2ᵗ, + c * w_1w_2 + s * w_3ᵗ, + 1.0F - c * (w_1² + w_3²), + c * w_2w_3 - s * w_1ᵗ, + c * w_1w_3 - s * w_2ᵗ, + c * w_2w_3 + s * w_1ᵗ, + 1.0F - c * (w_1² + w_2²), }}; } diff --git a/app/other/airbraker/tools/benchmark_airbrake_math.cpp b/app/other/airbraker/tools/benchmark_airbrake_math.cpp index 8d1de2e1a..b28c6aa4c 100644 --- a/app/other/airbraker/tools/benchmark_airbrake_math.cpp +++ b/app/other/airbraker/tools/benchmark_airbrake_math.cpp @@ -113,32 +113,32 @@ Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) { } Matrix3 ExpGyroNew(float w1, float w2, float w3, float t) { - const float wx = w1 * t; - const float wy = w2 * t; - const float wz = w3 * t; - - const float wx2 = wx * wx; - const float wy2 = wy * wy; - const float wz2 = wz * wz; - const float wxwy = wx * wy; - const float wxwz = wx * wz; - const float wywz = wy * wz; - - const float thetaSq = wx2 + wy2 + wz2; - const float theta = std::sqrt(thetaSq); - const float s = (theta == 0.0F) ? 1.0F : (std::sin(theta) / theta); - const float c = (theta == 0.0F) ? 0.0F : ((1.0F - std::cos(theta)) / thetaSq); + const float w₁ᵗ = w1 * t; + const float w₂ᵗ = w2 * t; + const float w₃ᵗ = w3 * t; + + const float w₁² = w₁ᵗ * w₁ᵗ; + const float w₂² = w₂ᵗ * w₂ᵗ; + const float w₃² = w₃ᵗ * w₃ᵗ; + const float w₁w₂ = w₁ᵗ * w₂ᵗ; + const float w₁w₃ = w₁ᵗ * w₃ᵗ; + const float w₂w₃ = w₂ᵗ * w₃ᵗ; + + const float θ² = w₁² + w₂² + w₃²; + const float θ = std::sqrt(θ²); + const float s = (θ == 0.0F) ? 1.0F : (std::sin(θ) / θ); + const float c = (θ == 0.0F) ? 0.0F : ((1.0F - std::cos(θ)) / θ²); return Matrix3{{ - 1.0F - c * (wy2 + wz2), - c * wxwy - s * wz, - c * wxwz + s * wy, - c * wxwy + s * wz, - 1.0F - c * (wx2 + wz2), - c * wywz - s * wx, - c * wxwz - s * wy, - c * wywz + s * wx, - 1.0F - c * (wx2 + wy2), + 1.0F - c * (w₂² + w₃²), + c * w₁w₂ - s * w₃ᵗ, + c * w₁w₃ + s * w₂ᵗ, + c * w₁w₂ + s * w₃ᵗ, + 1.0F - c * (w₁² + w₃²), + c * w₂w₃ - s * w₁ᵗ, + c * w₁w₃ - s * w₂ᵗ, + c * w₂w₃ + s * w₁ᵗ, + 1.0F - c * (w₁² + w₂²), }}; } From 61013a4d586bf42c96feb151788520b7fd81621e Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Mon, 13 Apr 2026 14:29:11 -0400 Subject: [PATCH 08/10] Make the benchmark use n_model --- app/other/airbraker/src/math/matrix.hpp | 2 - app/other/airbraker/src/n_model.cpp | 25 +- .../tools/benchmark_airbrake_math.cpp | 473 ++++++++---------- 3 files changed, 238 insertions(+), 262 deletions(-) diff --git a/app/other/airbraker/src/math/matrix.hpp b/app/other/airbraker/src/math/matrix.hpp index 7876fe040..f7092a2ef 100644 --- a/app/other/airbraker/src/math/matrix.hpp +++ b/app/other/airbraker/src/math/matrix.hpp @@ -12,7 +12,6 @@ template using Matrix = ManualMatrix; #endif -#include template Matrix matrixExpPowSeries(const Matrix &At, std::size_t iterations){ @@ -36,4 +35,3 @@ Matrix matrixExpPowSeries(const Matrix &At, std::size_t iterations){ } - diff --git a/app/other/airbraker/src/n_model.cpp b/app/other/airbraker/src/n_model.cpp index d6b0f4626..18934a6e0 100644 --- a/app/other/airbraker/src/n_model.cpp +++ b/app/other/airbraker/src/n_model.cpp @@ -1,8 +1,23 @@ +#ifndef AIRBRAKER_BENCHMARK #include "n_model.hpp" +#include "n_preboost.hpp" +#else +#include "math/matrix.hpp" +#include "n_autocoder_types.h" + +namespace NModel { +struct KalmanState { + float estAltitude; + float estVelocity; + float estAcceleration; + float estBias; +}; +void RotateRocketVectorToIMUVector(const NTypes::AccelerometerData& xyz, NTypes::AccelerometerData& out); +} // namespace NModel +#endif #include "math/matrix.hpp" #include "n_autocoder_types.h" -#include "n_preboost.hpp" #include "quantile_lut_data.h" #include @@ -234,6 +249,14 @@ void FeedGyro(uint32_t msSinceBoot, const NTypes::GyroscopeData& gyro) { int GetOrientation() { return 0; } bool EverWentOutOfBounds() { return everWentOutOfBounds; } +#ifdef AIRBRAKER_BENCHMARK +void ResetGyroStateForBenchmark() { + gyroOrientation = {1.0F, 0.0F, 0.0F, 0.0F}; + everWentOutOfBounds = false; + lastGyroIntegrationMsSinceBoot = 0; +} +#endif + void FillPacketWithOrientationMatrix(float* arr) { const float ww = gyroOrientation.w * gyroOrientation.w; const float xx = gyroOrientation.x * gyroOrientation.x; diff --git a/app/other/airbraker/tools/benchmark_airbrake_math.cpp b/app/other/airbraker/tools/benchmark_airbrake_math.cpp index b28c6aa4c..a9037226f 100644 --- a/app/other/airbraker/tools/benchmark_airbrake_math.cpp +++ b/app/other/airbraker/tools/benchmark_airbrake_math.cpp @@ -1,5 +1,7 @@ #include "../src/math/c_manualmatrix.hpp" +#include "quantile_lut_data.h" + #include #include #include @@ -9,67 +11,70 @@ #include #include +constexpr float ATMOSPHERE[] = {AUTOGEN_ATMOSPHERE_COEFFICIENTS}; + +#define AIRBRAKER_BENCHMARK 1 +#include "../src/n_model.cpp" + namespace { using Matrix3 = ManualMatrix<3, 3, float>; using Vector3 = ManualMatrix<3, 1, float>; -using ArrayVec3 = std::array; -struct Quaternion { +struct BenchQuaternion { float w; float x; float y; float z; }; -constexpr std::array kAtmosphere = { - 1.2345e2F, -1.876e-2F, 3.4e-6F, -5.1e-10F, 7.2e-14F, -3.3e-18F, 6.8e-23F, -}; - -constexpr std::array, 3> kImuToRocket = {{ - {0.9961947F, -0.0121320F, 0.0864101F}, - {0.0151344F, 0.9993007F, -0.0336292F}, - {-0.0858317F, 0.0349465F, 0.9956955F}, -}}; - template -inline void DoNotOptimize(T const& value) { +inline void DoNotOptimize(const T& value) +{ asm volatile("" : : "g"(value) : "memory"); } -float ClampUnit(float v) { return std::max(-1.0F, std::min(1.0F, v)); } +BenchQuaternion QuaternionMultiply(const BenchQuaternion& lhs, const BenchQuaternion& rhs) +{ + return { + .w = (lhs.w * rhs.w) - (lhs.x * rhs.x) - (lhs.y * rhs.y) - (lhs.z * rhs.z), + .x = (lhs.w * rhs.x) + (lhs.x * rhs.w) + (lhs.y * rhs.z) - (lhs.z * rhs.y), + .y = (lhs.w * rhs.y) - (lhs.x * rhs.z) + (lhs.y * rhs.w) + (lhs.z * rhs.x), + .z = (lhs.w * rhs.z) + (lhs.x * rhs.y) - (lhs.y * rhs.x) + (lhs.z * rhs.w), + }; +} -float Dot3(const Vector3& a, const Vector3& b) { - return (a.Get(0, 0) * b.Get(0, 0)) + (a.Get(1, 0) * b.Get(1, 0)) + (a.Get(2, 0) * b.Get(2, 0)); +BenchQuaternion QuaternionConjugate(const BenchQuaternion& q) +{ + return {.w = q.w, .x = -q.x, .y = -q.y, .z = -q.z}; } -float Dot3(const ArrayVec3& a, const ArrayVec3& b) { return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]); } +std::array RotateVectorByQuaternion(const BenchQuaternion& q, const std::array& v) +{ + const BenchQuaternion p{0.0F, v[0], v[1], v[2]}; + const BenchQuaternion qp = QuaternionMultiply(q, p); + const BenchQuaternion rotated = QuaternionMultiply(qp, QuaternionConjugate(q)); + return {rotated.x, rotated.y, rotated.z}; +} -ArrayVec3 Cross3(const ArrayVec3& a, const ArrayVec3& b) { - return { - (a[1] * b[2]) - (a[2] * b[1]), - (a[2] * b[0]) - (a[0] * b[2]), - (a[0] * b[1]) - (a[1] * b[0]), - }; +std::array RotateRocketToImu(const std::array& v) +{ + static constexpr BenchQuaternion kImuToRocket{AUTOGEN_IMU_TO_ROCKET_QUAT_INITIALIZER}; + return RotateVectorByQuaternion(QuaternionConjugate(kImuToRocket), v); } -std::array RotateImuToRocket(const std::array& v) { - std::array out{}; - for (int r = 0; r < 3; ++r) { - out[r] = (kImuToRocket[r][0] * v[0]) + (kImuToRocket[r][1] * v[1]) + (kImuToRocket[r][2] * v[2]); - } - return out; +float ClampUnit(float v) +{ + return std::max(-1.0F, std::min(1.0F, v)); } -std::array RotateRocketToImu(const std::array& v) { - std::array out{}; - for (int r = 0; r < 3; ++r) { - out[r] = (kImuToRocket[0][r] * v[0]) + (kImuToRocket[1][r] * v[1]) + (kImuToRocket[2][r] * v[2]); - } - return out; +float Dot3(const Vector3& a, const Vector3& b) +{ + return (a.Get(0, 0) * b.Get(0, 0)) + (a.Get(1, 0) * b.Get(1, 0)) + (a.Get(2, 0) * b.Get(2, 0)); } -Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) { +Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) +{ w1 *= t; w2 *= t; w3 *= t; @@ -82,26 +87,14 @@ Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) { const float w2w3 = w2 * w3; Matrix3 A{{ - 0, - -w3, - w2, - w3, - 0, - -w1, - -w2, - w1, - 0, + 0, -w3, w2, + w3, 0, -w1, + -w2, w1, 0, }}; Matrix3 A2{{ - -(w2_2 + w3_2), - w1w2, - w1w3, - w1w2, - -(w1_2 + w3_2), - w2w3, - w1w3, - w2w3, - -(w1_2 + w2_2), + -(w2_2 + w3_2), w1w2, w1w3, + w1w2, -(w1_2 + w3_2), w2w3, + w1w3, w2w3, -(w1_2 + w2_2), }}; const float normSq = w1_2 + w2_2 + w3_2; @@ -112,100 +105,17 @@ Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) { return Matrix3::Identity() + (A * s) + (A2 * c); } -Matrix3 ExpGyroNew(float w1, float w2, float w3, float t) { - const float w₁ᵗ = w1 * t; - const float w₂ᵗ = w2 * t; - const float w₃ᵗ = w3 * t; - - const float w₁² = w₁ᵗ * w₁ᵗ; - const float w₂² = w₂ᵗ * w₂ᵗ; - const float w₃² = w₃ᵗ * w₃ᵗ; - const float w₁w₂ = w₁ᵗ * w₂ᵗ; - const float w₁w₃ = w₁ᵗ * w₃ᵗ; - const float w₂w₃ = w₂ᵗ * w₃ᵗ; - - const float θ² = w₁² + w₂² + w₃²; - const float θ = std::sqrt(θ²); - const float s = (θ == 0.0F) ? 1.0F : (std::sin(θ) / θ); - const float c = (θ == 0.0F) ? 0.0F : ((1.0F - std::cos(θ)) / θ²); - - return Matrix3{{ - 1.0F - c * (w₂² + w₃²), - c * w₁w₂ - s * w₃ᵗ, - c * w₁w₃ + s * w₂ᵗ, - c * w₁w₂ + s * w₃ᵗ, - 1.0F - c * (w₁² + w₃²), - c * w₂w₃ - s * w₁ᵗ, - c * w₁w₃ - s * w₂ᵗ, - c * w₂w₃ + s * w₁ᵗ, - 1.0F - c * (w₁² + w₂²), - }}; -} - -float AltitudeOriginal(float kPa) { +float AltitudeOriginal(float kPa) +{ const float x = kPa * 1000.0F; float sum = 0.0F; float xn = x; - for (std::size_t i = 1; i < kAtmosphere.size(); ++i) { - sum += kAtmosphere[i] * xn; + for (size_t i = 1; i < AUTOGEN_ATMOSPHERE_NUM_COEFFECIENTS; ++i) + { + sum += ATMOSPHERE[i] * xn; xn *= x; } - return sum + kAtmosphere[0]; -} - -float AltitudeNew(float kPa) { - const float x = kPa * 1000.0F; - float sum = kAtmosphere[kAtmosphere.size() - 1]; - for (std::size_t i = kAtmosphere.size() - 1; i-- > 0;) { - sum = (sum * x) + kAtmosphere[i]; - } - return sum; -} - -Quaternion QuaternionMultiply(const Quaternion& lhs, const Quaternion& rhs) { - return { - (lhs.w * rhs.w) - (lhs.x * rhs.x) - (lhs.y * rhs.y) - (lhs.z * rhs.z), - (lhs.w * rhs.x) + (lhs.x * rhs.w) + (lhs.y * rhs.z) - (lhs.z * rhs.y), - (lhs.w * rhs.y) - (lhs.x * rhs.z) + (lhs.y * rhs.w) + (lhs.z * rhs.x), - (lhs.w * rhs.z) + (lhs.x * rhs.y) - (lhs.y * rhs.x) + (lhs.z * rhs.w), - }; -} - -Quaternion NormalizeQuaternion(const Quaternion& q) { - const float normSq = (q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z); - if (normSq == 0.0F) { - return {1.0F, 0.0F, 0.0F, 0.0F}; - } - - const float invNorm = 1.0F / std::sqrt(normSq); - return {q.w * invNorm, q.x * invNorm, q.y * invNorm, q.z * invNorm}; -} - -Quaternion QuaternionDeltaFromGyro(float gx, float gy, float gz, float dt) { - const float rx = gx * dt; - const float ry = gy * dt; - const float rz = gz * dt; - const float rotationMagnitudeSq = (rx * rx) + (ry * ry) + (rz * rz); - const float rotationMagnitude = std::sqrt(rotationMagnitudeSq); - - if (rotationMagnitude == 0.0F) { - return {1.0F, 0.0F, 0.0F, 0.0F}; - } - - const float halfAngle = 0.5F * rotationMagnitude; - const float vectorScale = std::sin(halfAngle) / rotationMagnitude; - return {std::cos(halfAngle), rx * vectorScale, ry * vectorScale, rz * vectorScale}; -} - -ArrayVec3 RotateVectorByQuaternion(const Quaternion& q, const ArrayVec3& v) { - const ArrayVec3 u{q.x, q.y, q.z}; - const ArrayVec3 uv = Cross3(u, v); - const ArrayVec3 uuv = Cross3(u, uv); - return { - v[0] + (2.0F * ((q.w * uv[0]) + uuv[0])), - v[1] + (2.0F * ((q.w * uv[1]) + uuv[1])), - v[2] + (2.0F * ((q.w * uv[2]) + uuv[2])), - }; + return sum + ATMOSPHERE[0]; } struct GyroStepResult { @@ -213,42 +123,36 @@ struct GyroStepResult { bool outOfBounds; }; -struct QuaternionGyroStepResult { - Quaternion orientation; - bool outOfBounds; -}; - -GyroStepResult FeedGyroOriginal(Matrix3 orientation, float gx, float gy, float gz, float dt) { +GyroStepResult FeedGyroOriginal(Matrix3 orientation, float gx, float gy, float gz, float dt) +{ const Matrix3 eAT = ExpGyroOriginal(gx, gy, gz, dt); orientation = orientation * eAT; const auto zInImu = RotateRocketToImu({0.0F, 0.0F, 1.0F}); const Vector3 initial{{zInImu[0], zInImu[1], zInImu[2]}}; const Vector3 now = orientation * initial; - const auto rotated = RotateImuToRocket({now.Get(0, 0), now.Get(1, 0), now.Get(2, 0)}); - - (void) rotated; - const float initialNorm = - std::sqrt((initial.Get(0, 0) * initial.Get(0, 0)) + (initial.Get(1, 0) * initial.Get(1, 0)) + - (initial.Get(2, 0) * initial.Get(2, 0))); + const float initialNorm = std::sqrt((initial.Get(0, 0) * initial.Get(0, 0)) + + (initial.Get(1, 0) * initial.Get(1, 0)) + + (initial.Get(2, 0) * initial.Get(2, 0))); const Vector3 normedNow = initial * (1.0F / initialNorm); - const float dot = (initial.Get(0, 0) * normedNow.Get(0, 0)) + (initial.Get(1, 0) * normedNow.Get(1, 0)) + + const float dot = (initial.Get(0, 0) * normedNow.Get(0, 0)) + + (initial.Get(1, 0) * normedNow.Get(1, 0)) + (initial.Get(2, 0) * normedNow.Get(2, 0)); const float offStart = std::acos(dot); return {orientation, offStart > (30.0F * 3.14159F / 180.0F)}; } -QuaternionGyroStepResult FeedGyroNew(Quaternion orientation, float gx, float gy, float gz, float dt) { - constexpr float kCosMaxTilt = 0.8660254037844386F; - static const ArrayVec3 initial = RotateRocketToImu({0.0F, 0.0F, 1.0F}); - - const Quaternion delta = QuaternionDeltaFromGyro(gx, gy, gz, dt); - orientation = NormalizeQuaternion(QuaternionMultiply(orientation, delta)); - - const ArrayVec3 now = RotateVectorByQuaternion(orientation, initial); - const float cosOffStart = ClampUnit(Dot3(initial, now)); - return {orientation, cosOffStart < kCosMaxTilt}; +double MaxOrientationDiff(const Matrix3& lhs, const float* rhs) +{ + double maxDiff = 0.0; + for (int i = 0; i < 9; ++i) + { + const int row = i / 3; + const int col = i % 3; + maxDiff = std::max(maxDiff, std::abs(static_cast(lhs.Get(row, col)) - static_cast(rhs[i]))); + } + return maxDiff; } struct Stats { @@ -257,10 +161,12 @@ struct Stats { }; template -Stats RunBenchmark(const char* name, Fn&& fn, std::size_t iterations, std::size_t rounds) { +Stats RunBenchmark(const char* name, Fn&& fn, size_t iterations, size_t rounds) +{ std::vector samples; samples.reserve(rounds); - for (std::size_t round = 0; round < rounds; ++round) { + for (size_t round = 0; round < rounds; ++round) + { const auto start = std::chrono::steady_clock::now(); fn(iterations); const auto end = std::chrono::steady_clock::now(); @@ -274,118 +180,167 @@ Stats RunBenchmark(const char* name, Fn&& fn, std::size_t iterations, std::size_ return {minNs, meanNs}; } +void RunBehaviorCase(const char* name, float gx, float gy, float gz, uint32_t dtMs, size_t steps) +{ + Matrix3 originalOrientation = Matrix3::Identity(); + bool oldEverOut = false; + int firstOldOutStep = -1; + int firstNewOutStep = -1; + double maxOrientationDiff = 0.0; + + NModel::ResetGyroStateForBenchmark(); + uint32_t timestampMs = 0; + for (size_t i = 0; i < steps; ++i) + { + const auto oldResult = FeedGyroOriginal(originalOrientation, gx, gy, gz, static_cast(dtMs) / 1000.0F); + originalOrientation = oldResult.orientation; + if ((firstOldOutStep < 0) && oldResult.outOfBounds) + { + firstOldOutStep = static_cast(i); + } + oldEverOut |= oldResult.outOfBounds; + + timestampMs += dtMs; + NModel::FeedGyro(timestampMs, {.X = gx, .Y = gy, .Z = gz}); + float orientationMatrix[9] = {}; + NModel::FillPacketWithOrientationMatrix(orientationMatrix); + maxOrientationDiff = std::max(maxOrientationDiff, MaxOrientationDiff(originalOrientation, orientationMatrix)); + if ((firstNewOutStep < 0) && NModel::EverWentOutOfBounds()) + { + firstNewOutStep = static_cast(i); + } + } + + std::printf("%s\n", name); + std::printf(" max orientation diff: %.6f\n", maxOrientationDiff); + std::printf(" first old out step: %d\n", firstOldOutStep); + std::printf(" first new out step: %d\n", firstNewOutStep); + DoNotOptimize(oldEverOut); +} + } // namespace -int main() { - constexpr std::size_t kIterations = 1'000'000; - constexpr std::size_t kRounds = 8; +namespace NModel { +void RotateRocketVectorToIMUVector(const NTypes::AccelerometerData& xyz, NTypes::AccelerometerData& out) +{ + const auto rotated = RotateRocketToImu({xyz.X, xyz.Y, xyz.Z}); + out.X = rotated[0]; + out.Y = rotated[1]; + out.Z = rotated[2]; +} +} // namespace NModel + +int main() +{ + constexpr size_t kIterations = 1'000'000; + constexpr size_t kRounds = 8; volatile float sink = 0.0F; volatile bool sinkBool = false; - for (std::size_t i = 0; i < 10000; ++i) { + for (size_t i = 0; i < 10000; ++i) + { const float gx = 0.02F + static_cast(i % 11) * 0.004F; const float gy = -0.03F + static_cast(i % 7) * 0.005F; const float gz = 0.01F + static_cast(i % 13) * 0.003F; sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); - sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); + sink += NModel::expGyro(gx, gy, gz, 0.01F).Get(0, 0); sink += AltitudeOriginal(85.0F + static_cast(i % 50) * 0.1F); - sink += AltitudeNew(85.0F + static_cast(i % 50) * 0.1F); + sink += NModel::AltitudeMetersFromPressureKPa(85.0F + static_cast(i % 50) * 0.1F); } std::puts("Airbraker math benchmark"); std::printf("iterations=%zu rounds=%zu\n\n", kIterations, kRounds); - const auto expOriginal = RunBenchmark( - "expGyro original", - [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); - } - DoNotOptimize(sink); - }, - kIterations, kRounds); - - const auto expNew = RunBenchmark( - "expGyro new", - [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); - } - DoNotOptimize(sink); - }, - kIterations, kRounds); - - const auto altOriginal = RunBenchmark( - "altitude original", - [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) { - sink += AltitudeOriginal(80.0F + static_cast(i % 1000) * 0.02F); - } - DoNotOptimize(sink); - }, - kIterations, kRounds); - - const auto altNew = RunBenchmark( - "altitude new", - [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) { - sink += AltitudeNew(80.0F + static_cast(i % 1000) * 0.02F); - } - DoNotOptimize(sink); - }, - kIterations, kRounds); - - const auto feedOriginal = RunBenchmark( - "feedGyro original", - [&](std::size_t iterations) { - Matrix3 orientation = Matrix3::Identity(); - bool everOut = false; - for (std::size_t i = 0; i < iterations; ++i) { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - const auto result = FeedGyroOriginal(orientation, gx, gy, gz, 0.01F); - orientation = result.orientation; - everOut |= result.outOfBounds; - } - sink += orientation.Get(0, 0); - sinkBool ^= everOut; - DoNotOptimize(sink); - DoNotOptimize(sinkBool); - }, - kIterations, kRounds); - - const auto feedNew = RunBenchmark( - "feedGyro new", - [&](std::size_t iterations) { - Quaternion orientation{1.0F, 0.0F, 0.0F, 0.0F}; - bool everOut = false; - for (std::size_t i = 0; i < iterations; ++i) { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - const auto result = FeedGyroNew(orientation, gx, gy, gz, 0.01F); - orientation = result.orientation; - everOut |= result.outOfBounds; - } - sink += orientation.w; - sinkBool ^= everOut; - DoNotOptimize(sink); - DoNotOptimize(sinkBool); - }, - kIterations, kRounds); + const auto expOriginal = RunBenchmark("expGyro original", [&](size_t iterations) { + for (size_t i = 0; i < iterations; ++i) + { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); + } + DoNotOptimize(sink); + }, kIterations, kRounds); + + const auto expNew = RunBenchmark("expGyro new", [&](size_t iterations) { + for (size_t i = 0; i < iterations; ++i) + { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + sink += NModel::expGyro(gx, gy, gz, 0.01F).Get(0, 0); + } + DoNotOptimize(sink); + }, kIterations, kRounds); + + const auto altOriginal = RunBenchmark("altitude original", [&](size_t iterations) { + for (size_t i = 0; i < iterations; ++i) + { + sink += AltitudeOriginal(80.0F + static_cast(i % 1000) * 0.02F); + } + DoNotOptimize(sink); + }, kIterations, kRounds); + + const auto altNew = RunBenchmark("altitude new", [&](size_t iterations) { + for (size_t i = 0; i < iterations; ++i) + { + sink += NModel::AltitudeMetersFromPressureKPa(80.0F + static_cast(i % 1000) * 0.02F); + } + DoNotOptimize(sink); + }, kIterations, kRounds); + + const auto feedOriginal = RunBenchmark("feedGyro original", [&](size_t iterations) { + Matrix3 orientation = Matrix3::Identity(); + bool everOut = false; + for (size_t i = 0; i < iterations; ++i) + { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + const auto result = FeedGyroOriginal(orientation, gx, gy, gz, 0.01F); + orientation = result.orientation; + everOut |= result.outOfBounds; + } + sink += orientation.Get(0, 0); + sinkBool ^= everOut; + DoNotOptimize(sink); + DoNotOptimize(sinkBool); + }, kIterations, kRounds); + + const auto feedNew = RunBenchmark("feedGyro new", [&](size_t iterations) { + NModel::ResetGyroStateForBenchmark(); + bool everOut = false; + uint32_t timestampMs = 0; + float orientationMatrix[9] = {}; + for (size_t i = 0; i < iterations; ++i) + { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + timestampMs += 10; + NModel::FeedGyro(timestampMs, {.X = gx, .Y = gy, .Z = gz}); + everOut |= NModel::EverWentOutOfBounds(); + } + NModel::FillPacketWithOrientationMatrix(orientationMatrix); + sink += orientationMatrix[0]; + sinkBool ^= everOut; + DoNotOptimize(sink); + DoNotOptimize(sinkBool); + }, kIterations, kRounds); std::puts("\nSpeedups based on best run"); std::printf("expGyro: %.2fx\n", expOriginal.minNs / expNew.minNs); std::printf("altitude: %.2fx\n", altOriginal.minNs / altNew.minNs); std::printf("feedGyro step: %.2fx\n", feedOriginal.minNs / feedNew.minNs); + + std::puts("\nBehavior checks"); + RunBehaviorCase("zero gyro", 0.0F, 0.0F, 0.0F, 10, 800); + RunBehaviorCase("constant pitch", 0.10F, 0.0F, 0.0F, 10, 800); + const auto rocketAxisInImu = RotateRocketToImu({0.0F, 0.0F, 1.0F}); + RunBehaviorCase("spin about rocket axis", rocketAxisInImu[0] * 0.10F, rocketAxisInImu[1] * 0.10F, + rocketAxisInImu[2] * 0.10F, 10, 800); + std::printf("\nignore sinks: %f %d\n", sink, static_cast(sinkBool)); return 0; } From 5cdb3983ce879d2fc655d0a9a2dbfc685ac51029 Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Mon, 13 Apr 2026 14:44:36 -0400 Subject: [PATCH 09/10] Revert "Make the benchmark use n_model" This reverts commit 61013a4d586bf42c96feb151788520b7fd81621e. --- app/other/airbraker/src/math/matrix.hpp | 2 + app/other/airbraker/src/n_model.cpp | 25 +- .../tools/benchmark_airbrake_math.cpp | 473 ++++++++++-------- 3 files changed, 262 insertions(+), 238 deletions(-) diff --git a/app/other/airbraker/src/math/matrix.hpp b/app/other/airbraker/src/math/matrix.hpp index f7092a2ef..7876fe040 100644 --- a/app/other/airbraker/src/math/matrix.hpp +++ b/app/other/airbraker/src/math/matrix.hpp @@ -12,6 +12,7 @@ template using Matrix = ManualMatrix; #endif +#include template Matrix matrixExpPowSeries(const Matrix &At, std::size_t iterations){ @@ -35,3 +36,4 @@ Matrix matrixExpPowSeries(const Matrix &At, std::size_t iterations){ } + diff --git a/app/other/airbraker/src/n_model.cpp b/app/other/airbraker/src/n_model.cpp index 18934a6e0..d6b0f4626 100644 --- a/app/other/airbraker/src/n_model.cpp +++ b/app/other/airbraker/src/n_model.cpp @@ -1,23 +1,8 @@ -#ifndef AIRBRAKER_BENCHMARK #include "n_model.hpp" -#include "n_preboost.hpp" -#else -#include "math/matrix.hpp" -#include "n_autocoder_types.h" - -namespace NModel { -struct KalmanState { - float estAltitude; - float estVelocity; - float estAcceleration; - float estBias; -}; -void RotateRocketVectorToIMUVector(const NTypes::AccelerometerData& xyz, NTypes::AccelerometerData& out); -} // namespace NModel -#endif #include "math/matrix.hpp" #include "n_autocoder_types.h" +#include "n_preboost.hpp" #include "quantile_lut_data.h" #include @@ -249,14 +234,6 @@ void FeedGyro(uint32_t msSinceBoot, const NTypes::GyroscopeData& gyro) { int GetOrientation() { return 0; } bool EverWentOutOfBounds() { return everWentOutOfBounds; } -#ifdef AIRBRAKER_BENCHMARK -void ResetGyroStateForBenchmark() { - gyroOrientation = {1.0F, 0.0F, 0.0F, 0.0F}; - everWentOutOfBounds = false; - lastGyroIntegrationMsSinceBoot = 0; -} -#endif - void FillPacketWithOrientationMatrix(float* arr) { const float ww = gyroOrientation.w * gyroOrientation.w; const float xx = gyroOrientation.x * gyroOrientation.x; diff --git a/app/other/airbraker/tools/benchmark_airbrake_math.cpp b/app/other/airbraker/tools/benchmark_airbrake_math.cpp index a9037226f..b28c6aa4c 100644 --- a/app/other/airbraker/tools/benchmark_airbrake_math.cpp +++ b/app/other/airbraker/tools/benchmark_airbrake_math.cpp @@ -1,7 +1,5 @@ #include "../src/math/c_manualmatrix.hpp" -#include "quantile_lut_data.h" - #include #include #include @@ -11,70 +9,67 @@ #include #include -constexpr float ATMOSPHERE[] = {AUTOGEN_ATMOSPHERE_COEFFICIENTS}; - -#define AIRBRAKER_BENCHMARK 1 -#include "../src/n_model.cpp" - namespace { using Matrix3 = ManualMatrix<3, 3, float>; using Vector3 = ManualMatrix<3, 1, float>; +using ArrayVec3 = std::array; -struct BenchQuaternion { +struct Quaternion { float w; float x; float y; float z; }; +constexpr std::array kAtmosphere = { + 1.2345e2F, -1.876e-2F, 3.4e-6F, -5.1e-10F, 7.2e-14F, -3.3e-18F, 6.8e-23F, +}; + +constexpr std::array, 3> kImuToRocket = {{ + {0.9961947F, -0.0121320F, 0.0864101F}, + {0.0151344F, 0.9993007F, -0.0336292F}, + {-0.0858317F, 0.0349465F, 0.9956955F}, +}}; + template -inline void DoNotOptimize(const T& value) -{ +inline void DoNotOptimize(T const& value) { asm volatile("" : : "g"(value) : "memory"); } -BenchQuaternion QuaternionMultiply(const BenchQuaternion& lhs, const BenchQuaternion& rhs) -{ - return { - .w = (lhs.w * rhs.w) - (lhs.x * rhs.x) - (lhs.y * rhs.y) - (lhs.z * rhs.z), - .x = (lhs.w * rhs.x) + (lhs.x * rhs.w) + (lhs.y * rhs.z) - (lhs.z * rhs.y), - .y = (lhs.w * rhs.y) - (lhs.x * rhs.z) + (lhs.y * rhs.w) + (lhs.z * rhs.x), - .z = (lhs.w * rhs.z) + (lhs.x * rhs.y) - (lhs.y * rhs.x) + (lhs.z * rhs.w), - }; -} +float ClampUnit(float v) { return std::max(-1.0F, std::min(1.0F, v)); } -BenchQuaternion QuaternionConjugate(const BenchQuaternion& q) -{ - return {.w = q.w, .x = -q.x, .y = -q.y, .z = -q.z}; +float Dot3(const Vector3& a, const Vector3& b) { + return (a.Get(0, 0) * b.Get(0, 0)) + (a.Get(1, 0) * b.Get(1, 0)) + (a.Get(2, 0) * b.Get(2, 0)); } -std::array RotateVectorByQuaternion(const BenchQuaternion& q, const std::array& v) -{ - const BenchQuaternion p{0.0F, v[0], v[1], v[2]}; - const BenchQuaternion qp = QuaternionMultiply(q, p); - const BenchQuaternion rotated = QuaternionMultiply(qp, QuaternionConjugate(q)); - return {rotated.x, rotated.y, rotated.z}; -} +float Dot3(const ArrayVec3& a, const ArrayVec3& b) { return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]); } -std::array RotateRocketToImu(const std::array& v) -{ - static constexpr BenchQuaternion kImuToRocket{AUTOGEN_IMU_TO_ROCKET_QUAT_INITIALIZER}; - return RotateVectorByQuaternion(QuaternionConjugate(kImuToRocket), v); +ArrayVec3 Cross3(const ArrayVec3& a, const ArrayVec3& b) { + return { + (a[1] * b[2]) - (a[2] * b[1]), + (a[2] * b[0]) - (a[0] * b[2]), + (a[0] * b[1]) - (a[1] * b[0]), + }; } -float ClampUnit(float v) -{ - return std::max(-1.0F, std::min(1.0F, v)); +std::array RotateImuToRocket(const std::array& v) { + std::array out{}; + for (int r = 0; r < 3; ++r) { + out[r] = (kImuToRocket[r][0] * v[0]) + (kImuToRocket[r][1] * v[1]) + (kImuToRocket[r][2] * v[2]); + } + return out; } -float Dot3(const Vector3& a, const Vector3& b) -{ - return (a.Get(0, 0) * b.Get(0, 0)) + (a.Get(1, 0) * b.Get(1, 0)) + (a.Get(2, 0) * b.Get(2, 0)); +std::array RotateRocketToImu(const std::array& v) { + std::array out{}; + for (int r = 0; r < 3; ++r) { + out[r] = (kImuToRocket[0][r] * v[0]) + (kImuToRocket[1][r] * v[1]) + (kImuToRocket[2][r] * v[2]); + } + return out; } -Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) -{ +Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) { w1 *= t; w2 *= t; w3 *= t; @@ -87,14 +82,26 @@ Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) const float w2w3 = w2 * w3; Matrix3 A{{ - 0, -w3, w2, - w3, 0, -w1, - -w2, w1, 0, + 0, + -w3, + w2, + w3, + 0, + -w1, + -w2, + w1, + 0, }}; Matrix3 A2{{ - -(w2_2 + w3_2), w1w2, w1w3, - w1w2, -(w1_2 + w3_2), w2w3, - w1w3, w2w3, -(w1_2 + w2_2), + -(w2_2 + w3_2), + w1w2, + w1w3, + w1w2, + -(w1_2 + w3_2), + w2w3, + w1w3, + w2w3, + -(w1_2 + w2_2), }}; const float normSq = w1_2 + w2_2 + w3_2; @@ -105,17 +112,100 @@ Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) return Matrix3::Identity() + (A * s) + (A2 * c); } -float AltitudeOriginal(float kPa) -{ +Matrix3 ExpGyroNew(float w1, float w2, float w3, float t) { + const float w₁ᵗ = w1 * t; + const float w₂ᵗ = w2 * t; + const float w₃ᵗ = w3 * t; + + const float w₁² = w₁ᵗ * w₁ᵗ; + const float w₂² = w₂ᵗ * w₂ᵗ; + const float w₃² = w₃ᵗ * w₃ᵗ; + const float w₁w₂ = w₁ᵗ * w₂ᵗ; + const float w₁w₃ = w₁ᵗ * w₃ᵗ; + const float w₂w₃ = w₂ᵗ * w₃ᵗ; + + const float θ² = w₁² + w₂² + w₃²; + const float θ = std::sqrt(θ²); + const float s = (θ == 0.0F) ? 1.0F : (std::sin(θ) / θ); + const float c = (θ == 0.0F) ? 0.0F : ((1.0F - std::cos(θ)) / θ²); + + return Matrix3{{ + 1.0F - c * (w₂² + w₃²), + c * w₁w₂ - s * w₃ᵗ, + c * w₁w₃ + s * w₂ᵗ, + c * w₁w₂ + s * w₃ᵗ, + 1.0F - c * (w₁² + w₃²), + c * w₂w₃ - s * w₁ᵗ, + c * w₁w₃ - s * w₂ᵗ, + c * w₂w₃ + s * w₁ᵗ, + 1.0F - c * (w₁² + w₂²), + }}; +} + +float AltitudeOriginal(float kPa) { const float x = kPa * 1000.0F; float sum = 0.0F; float xn = x; - for (size_t i = 1; i < AUTOGEN_ATMOSPHERE_NUM_COEFFECIENTS; ++i) - { - sum += ATMOSPHERE[i] * xn; + for (std::size_t i = 1; i < kAtmosphere.size(); ++i) { + sum += kAtmosphere[i] * xn; xn *= x; } - return sum + ATMOSPHERE[0]; + return sum + kAtmosphere[0]; +} + +float AltitudeNew(float kPa) { + const float x = kPa * 1000.0F; + float sum = kAtmosphere[kAtmosphere.size() - 1]; + for (std::size_t i = kAtmosphere.size() - 1; i-- > 0;) { + sum = (sum * x) + kAtmosphere[i]; + } + return sum; +} + +Quaternion QuaternionMultiply(const Quaternion& lhs, const Quaternion& rhs) { + return { + (lhs.w * rhs.w) - (lhs.x * rhs.x) - (lhs.y * rhs.y) - (lhs.z * rhs.z), + (lhs.w * rhs.x) + (lhs.x * rhs.w) + (lhs.y * rhs.z) - (lhs.z * rhs.y), + (lhs.w * rhs.y) - (lhs.x * rhs.z) + (lhs.y * rhs.w) + (lhs.z * rhs.x), + (lhs.w * rhs.z) + (lhs.x * rhs.y) - (lhs.y * rhs.x) + (lhs.z * rhs.w), + }; +} + +Quaternion NormalizeQuaternion(const Quaternion& q) { + const float normSq = (q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z); + if (normSq == 0.0F) { + return {1.0F, 0.0F, 0.0F, 0.0F}; + } + + const float invNorm = 1.0F / std::sqrt(normSq); + return {q.w * invNorm, q.x * invNorm, q.y * invNorm, q.z * invNorm}; +} + +Quaternion QuaternionDeltaFromGyro(float gx, float gy, float gz, float dt) { + const float rx = gx * dt; + const float ry = gy * dt; + const float rz = gz * dt; + const float rotationMagnitudeSq = (rx * rx) + (ry * ry) + (rz * rz); + const float rotationMagnitude = std::sqrt(rotationMagnitudeSq); + + if (rotationMagnitude == 0.0F) { + return {1.0F, 0.0F, 0.0F, 0.0F}; + } + + const float halfAngle = 0.5F * rotationMagnitude; + const float vectorScale = std::sin(halfAngle) / rotationMagnitude; + return {std::cos(halfAngle), rx * vectorScale, ry * vectorScale, rz * vectorScale}; +} + +ArrayVec3 RotateVectorByQuaternion(const Quaternion& q, const ArrayVec3& v) { + const ArrayVec3 u{q.x, q.y, q.z}; + const ArrayVec3 uv = Cross3(u, v); + const ArrayVec3 uuv = Cross3(u, uv); + return { + v[0] + (2.0F * ((q.w * uv[0]) + uuv[0])), + v[1] + (2.0F * ((q.w * uv[1]) + uuv[1])), + v[2] + (2.0F * ((q.w * uv[2]) + uuv[2])), + }; } struct GyroStepResult { @@ -123,36 +213,42 @@ struct GyroStepResult { bool outOfBounds; }; -GyroStepResult FeedGyroOriginal(Matrix3 orientation, float gx, float gy, float gz, float dt) -{ +struct QuaternionGyroStepResult { + Quaternion orientation; + bool outOfBounds; +}; + +GyroStepResult FeedGyroOriginal(Matrix3 orientation, float gx, float gy, float gz, float dt) { const Matrix3 eAT = ExpGyroOriginal(gx, gy, gz, dt); orientation = orientation * eAT; const auto zInImu = RotateRocketToImu({0.0F, 0.0F, 1.0F}); const Vector3 initial{{zInImu[0], zInImu[1], zInImu[2]}}; const Vector3 now = orientation * initial; + const auto rotated = RotateImuToRocket({now.Get(0, 0), now.Get(1, 0), now.Get(2, 0)}); + + (void) rotated; - const float initialNorm = std::sqrt((initial.Get(0, 0) * initial.Get(0, 0)) + - (initial.Get(1, 0) * initial.Get(1, 0)) + - (initial.Get(2, 0) * initial.Get(2, 0))); + const float initialNorm = + std::sqrt((initial.Get(0, 0) * initial.Get(0, 0)) + (initial.Get(1, 0) * initial.Get(1, 0)) + + (initial.Get(2, 0) * initial.Get(2, 0))); const Vector3 normedNow = initial * (1.0F / initialNorm); - const float dot = (initial.Get(0, 0) * normedNow.Get(0, 0)) + - (initial.Get(1, 0) * normedNow.Get(1, 0)) + + const float dot = (initial.Get(0, 0) * normedNow.Get(0, 0)) + (initial.Get(1, 0) * normedNow.Get(1, 0)) + (initial.Get(2, 0) * normedNow.Get(2, 0)); const float offStart = std::acos(dot); return {orientation, offStart > (30.0F * 3.14159F / 180.0F)}; } -double MaxOrientationDiff(const Matrix3& lhs, const float* rhs) -{ - double maxDiff = 0.0; - for (int i = 0; i < 9; ++i) - { - const int row = i / 3; - const int col = i % 3; - maxDiff = std::max(maxDiff, std::abs(static_cast(lhs.Get(row, col)) - static_cast(rhs[i]))); - } - return maxDiff; +QuaternionGyroStepResult FeedGyroNew(Quaternion orientation, float gx, float gy, float gz, float dt) { + constexpr float kCosMaxTilt = 0.8660254037844386F; + static const ArrayVec3 initial = RotateRocketToImu({0.0F, 0.0F, 1.0F}); + + const Quaternion delta = QuaternionDeltaFromGyro(gx, gy, gz, dt); + orientation = NormalizeQuaternion(QuaternionMultiply(orientation, delta)); + + const ArrayVec3 now = RotateVectorByQuaternion(orientation, initial); + const float cosOffStart = ClampUnit(Dot3(initial, now)); + return {orientation, cosOffStart < kCosMaxTilt}; } struct Stats { @@ -161,12 +257,10 @@ struct Stats { }; template -Stats RunBenchmark(const char* name, Fn&& fn, size_t iterations, size_t rounds) -{ +Stats RunBenchmark(const char* name, Fn&& fn, std::size_t iterations, std::size_t rounds) { std::vector samples; samples.reserve(rounds); - for (size_t round = 0; round < rounds; ++round) - { + for (std::size_t round = 0; round < rounds; ++round) { const auto start = std::chrono::steady_clock::now(); fn(iterations); const auto end = std::chrono::steady_clock::now(); @@ -180,167 +274,118 @@ Stats RunBenchmark(const char* name, Fn&& fn, size_t iterations, size_t rounds) return {minNs, meanNs}; } -void RunBehaviorCase(const char* name, float gx, float gy, float gz, uint32_t dtMs, size_t steps) -{ - Matrix3 originalOrientation = Matrix3::Identity(); - bool oldEverOut = false; - int firstOldOutStep = -1; - int firstNewOutStep = -1; - double maxOrientationDiff = 0.0; - - NModel::ResetGyroStateForBenchmark(); - uint32_t timestampMs = 0; - for (size_t i = 0; i < steps; ++i) - { - const auto oldResult = FeedGyroOriginal(originalOrientation, gx, gy, gz, static_cast(dtMs) / 1000.0F); - originalOrientation = oldResult.orientation; - if ((firstOldOutStep < 0) && oldResult.outOfBounds) - { - firstOldOutStep = static_cast(i); - } - oldEverOut |= oldResult.outOfBounds; - - timestampMs += dtMs; - NModel::FeedGyro(timestampMs, {.X = gx, .Y = gy, .Z = gz}); - float orientationMatrix[9] = {}; - NModel::FillPacketWithOrientationMatrix(orientationMatrix); - maxOrientationDiff = std::max(maxOrientationDiff, MaxOrientationDiff(originalOrientation, orientationMatrix)); - if ((firstNewOutStep < 0) && NModel::EverWentOutOfBounds()) - { - firstNewOutStep = static_cast(i); - } - } - - std::printf("%s\n", name); - std::printf(" max orientation diff: %.6f\n", maxOrientationDiff); - std::printf(" first old out step: %d\n", firstOldOutStep); - std::printf(" first new out step: %d\n", firstNewOutStep); - DoNotOptimize(oldEverOut); -} - } // namespace -namespace NModel { -void RotateRocketVectorToIMUVector(const NTypes::AccelerometerData& xyz, NTypes::AccelerometerData& out) -{ - const auto rotated = RotateRocketToImu({xyz.X, xyz.Y, xyz.Z}); - out.X = rotated[0]; - out.Y = rotated[1]; - out.Z = rotated[2]; -} -} // namespace NModel - -int main() -{ - constexpr size_t kIterations = 1'000'000; - constexpr size_t kRounds = 8; +int main() { + constexpr std::size_t kIterations = 1'000'000; + constexpr std::size_t kRounds = 8; volatile float sink = 0.0F; volatile bool sinkBool = false; - for (size_t i = 0; i < 10000; ++i) - { + for (std::size_t i = 0; i < 10000; ++i) { const float gx = 0.02F + static_cast(i % 11) * 0.004F; const float gy = -0.03F + static_cast(i % 7) * 0.005F; const float gz = 0.01F + static_cast(i % 13) * 0.003F; sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); - sink += NModel::expGyro(gx, gy, gz, 0.01F).Get(0, 0); + sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); sink += AltitudeOriginal(85.0F + static_cast(i % 50) * 0.1F); - sink += NModel::AltitudeMetersFromPressureKPa(85.0F + static_cast(i % 50) * 0.1F); + sink += AltitudeNew(85.0F + static_cast(i % 50) * 0.1F); } std::puts("Airbraker math benchmark"); std::printf("iterations=%zu rounds=%zu\n\n", kIterations, kRounds); - const auto expOriginal = RunBenchmark("expGyro original", [&](size_t iterations) { - for (size_t i = 0; i < iterations; ++i) - { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); - } - DoNotOptimize(sink); - }, kIterations, kRounds); - - const auto expNew = RunBenchmark("expGyro new", [&](size_t iterations) { - for (size_t i = 0; i < iterations; ++i) - { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - sink += NModel::expGyro(gx, gy, gz, 0.01F).Get(0, 0); - } - DoNotOptimize(sink); - }, kIterations, kRounds); - - const auto altOriginal = RunBenchmark("altitude original", [&](size_t iterations) { - for (size_t i = 0; i < iterations; ++i) - { - sink += AltitudeOriginal(80.0F + static_cast(i % 1000) * 0.02F); - } - DoNotOptimize(sink); - }, kIterations, kRounds); - - const auto altNew = RunBenchmark("altitude new", [&](size_t iterations) { - for (size_t i = 0; i < iterations; ++i) - { - sink += NModel::AltitudeMetersFromPressureKPa(80.0F + static_cast(i % 1000) * 0.02F); - } - DoNotOptimize(sink); - }, kIterations, kRounds); - - const auto feedOriginal = RunBenchmark("feedGyro original", [&](size_t iterations) { - Matrix3 orientation = Matrix3::Identity(); - bool everOut = false; - for (size_t i = 0; i < iterations; ++i) - { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - const auto result = FeedGyroOriginal(orientation, gx, gy, gz, 0.01F); - orientation = result.orientation; - everOut |= result.outOfBounds; - } - sink += orientation.Get(0, 0); - sinkBool ^= everOut; - DoNotOptimize(sink); - DoNotOptimize(sinkBool); - }, kIterations, kRounds); - - const auto feedNew = RunBenchmark("feedGyro new", [&](size_t iterations) { - NModel::ResetGyroStateForBenchmark(); - bool everOut = false; - uint32_t timestampMs = 0; - float orientationMatrix[9] = {}; - for (size_t i = 0; i < iterations; ++i) - { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - timestampMs += 10; - NModel::FeedGyro(timestampMs, {.X = gx, .Y = gy, .Z = gz}); - everOut |= NModel::EverWentOutOfBounds(); - } - NModel::FillPacketWithOrientationMatrix(orientationMatrix); - sink += orientationMatrix[0]; - sinkBool ^= everOut; - DoNotOptimize(sink); - DoNotOptimize(sinkBool); - }, kIterations, kRounds); + const auto expOriginal = RunBenchmark( + "expGyro original", + [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); + } + DoNotOptimize(sink); + }, + kIterations, kRounds); + + const auto expNew = RunBenchmark( + "expGyro new", + [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); + } + DoNotOptimize(sink); + }, + kIterations, kRounds); + + const auto altOriginal = RunBenchmark( + "altitude original", + [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) { + sink += AltitudeOriginal(80.0F + static_cast(i % 1000) * 0.02F); + } + DoNotOptimize(sink); + }, + kIterations, kRounds); + + const auto altNew = RunBenchmark( + "altitude new", + [&](std::size_t iterations) { + for (std::size_t i = 0; i < iterations; ++i) { + sink += AltitudeNew(80.0F + static_cast(i % 1000) * 0.02F); + } + DoNotOptimize(sink); + }, + kIterations, kRounds); + + const auto feedOriginal = RunBenchmark( + "feedGyro original", + [&](std::size_t iterations) { + Matrix3 orientation = Matrix3::Identity(); + bool everOut = false; + for (std::size_t i = 0; i < iterations; ++i) { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + const auto result = FeedGyroOriginal(orientation, gx, gy, gz, 0.01F); + orientation = result.orientation; + everOut |= result.outOfBounds; + } + sink += orientation.Get(0, 0); + sinkBool ^= everOut; + DoNotOptimize(sink); + DoNotOptimize(sinkBool); + }, + kIterations, kRounds); + + const auto feedNew = RunBenchmark( + "feedGyro new", + [&](std::size_t iterations) { + Quaternion orientation{1.0F, 0.0F, 0.0F, 0.0F}; + bool everOut = false; + for (std::size_t i = 0; i < iterations; ++i) { + const float gx = 0.02F + static_cast(i % 11) * 0.004F; + const float gy = -0.03F + static_cast(i % 7) * 0.005F; + const float gz = 0.01F + static_cast(i % 13) * 0.003F; + const auto result = FeedGyroNew(orientation, gx, gy, gz, 0.01F); + orientation = result.orientation; + everOut |= result.outOfBounds; + } + sink += orientation.w; + sinkBool ^= everOut; + DoNotOptimize(sink); + DoNotOptimize(sinkBool); + }, + kIterations, kRounds); std::puts("\nSpeedups based on best run"); std::printf("expGyro: %.2fx\n", expOriginal.minNs / expNew.minNs); std::printf("altitude: %.2fx\n", altOriginal.minNs / altNew.minNs); std::printf("feedGyro step: %.2fx\n", feedOriginal.minNs / feedNew.minNs); - - std::puts("\nBehavior checks"); - RunBehaviorCase("zero gyro", 0.0F, 0.0F, 0.0F, 10, 800); - RunBehaviorCase("constant pitch", 0.10F, 0.0F, 0.0F, 10, 800); - const auto rocketAxisInImu = RotateRocketToImu({0.0F, 0.0F, 1.0F}); - RunBehaviorCase("spin about rocket axis", rocketAxisInImu[0] * 0.10F, rocketAxisInImu[1] * 0.10F, - rocketAxisInImu[2] * 0.10F, 10, 800); - std::printf("\nignore sinks: %f %d\n", sink, static_cast(sinkBool)); return 0; } From c93f071354621a0336dc752cc35b7eba19b91cd6 Mon Sep 17 00:00:00 2001 From: Aaron Chan Date: Mon, 13 Apr 2026 14:56:49 -0400 Subject: [PATCH 10/10] fmt --- .../tools/benchmark_airbrake_math.cpp | 391 ------------------ 1 file changed, 391 deletions(-) delete mode 100644 app/other/airbraker/tools/benchmark_airbrake_math.cpp diff --git a/app/other/airbraker/tools/benchmark_airbrake_math.cpp b/app/other/airbraker/tools/benchmark_airbrake_math.cpp deleted file mode 100644 index b28c6aa4c..000000000 --- a/app/other/airbraker/tools/benchmark_airbrake_math.cpp +++ /dev/null @@ -1,391 +0,0 @@ -#include "../src/math/c_manualmatrix.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace { - -using Matrix3 = ManualMatrix<3, 3, float>; -using Vector3 = ManualMatrix<3, 1, float>; -using ArrayVec3 = std::array; - -struct Quaternion { - float w; - float x; - float y; - float z; -}; - -constexpr std::array kAtmosphere = { - 1.2345e2F, -1.876e-2F, 3.4e-6F, -5.1e-10F, 7.2e-14F, -3.3e-18F, 6.8e-23F, -}; - -constexpr std::array, 3> kImuToRocket = {{ - {0.9961947F, -0.0121320F, 0.0864101F}, - {0.0151344F, 0.9993007F, -0.0336292F}, - {-0.0858317F, 0.0349465F, 0.9956955F}, -}}; - -template -inline void DoNotOptimize(T const& value) { - asm volatile("" : : "g"(value) : "memory"); -} - -float ClampUnit(float v) { return std::max(-1.0F, std::min(1.0F, v)); } - -float Dot3(const Vector3& a, const Vector3& b) { - return (a.Get(0, 0) * b.Get(0, 0)) + (a.Get(1, 0) * b.Get(1, 0)) + (a.Get(2, 0) * b.Get(2, 0)); -} - -float Dot3(const ArrayVec3& a, const ArrayVec3& b) { return (a[0] * b[0]) + (a[1] * b[1]) + (a[2] * b[2]); } - -ArrayVec3 Cross3(const ArrayVec3& a, const ArrayVec3& b) { - return { - (a[1] * b[2]) - (a[2] * b[1]), - (a[2] * b[0]) - (a[0] * b[2]), - (a[0] * b[1]) - (a[1] * b[0]), - }; -} - -std::array RotateImuToRocket(const std::array& v) { - std::array out{}; - for (int r = 0; r < 3; ++r) { - out[r] = (kImuToRocket[r][0] * v[0]) + (kImuToRocket[r][1] * v[1]) + (kImuToRocket[r][2] * v[2]); - } - return out; -} - -std::array RotateRocketToImu(const std::array& v) { - std::array out{}; - for (int r = 0; r < 3; ++r) { - out[r] = (kImuToRocket[0][r] * v[0]) + (kImuToRocket[1][r] * v[1]) + (kImuToRocket[2][r] * v[2]); - } - return out; -} - -Matrix3 ExpGyroOriginal(float w1, float w2, float w3, float t) { - w1 *= t; - w2 *= t; - w3 *= t; - - const float w1_2 = w1 * w1; - const float w2_2 = w2 * w2; - const float w3_2 = w3 * w3; - const float w1w2 = w1 * w2; - const float w1w3 = w1 * w3; - const float w2w3 = w2 * w3; - - Matrix3 A{{ - 0, - -w3, - w2, - w3, - 0, - -w1, - -w2, - w1, - 0, - }}; - Matrix3 A2{{ - -(w2_2 + w3_2), - w1w2, - w1w3, - w1w2, - -(w1_2 + w3_2), - w2w3, - w1w3, - w2w3, - -(w1_2 + w2_2), - }}; - - const float normSq = w1_2 + w2_2 + w3_2; - const float norm = std::sqrt(normSq); - const float s = (norm == 0.0F) ? 1.0F : (std::sin(norm) / norm); - const float c = (norm == 0.0F) ? 0.0F : ((1.0F - std::cos(norm)) / normSq); - - return Matrix3::Identity() + (A * s) + (A2 * c); -} - -Matrix3 ExpGyroNew(float w1, float w2, float w3, float t) { - const float w₁ᵗ = w1 * t; - const float w₂ᵗ = w2 * t; - const float w₃ᵗ = w3 * t; - - const float w₁² = w₁ᵗ * w₁ᵗ; - const float w₂² = w₂ᵗ * w₂ᵗ; - const float w₃² = w₃ᵗ * w₃ᵗ; - const float w₁w₂ = w₁ᵗ * w₂ᵗ; - const float w₁w₃ = w₁ᵗ * w₃ᵗ; - const float w₂w₃ = w₂ᵗ * w₃ᵗ; - - const float θ² = w₁² + w₂² + w₃²; - const float θ = std::sqrt(θ²); - const float s = (θ == 0.0F) ? 1.0F : (std::sin(θ) / θ); - const float c = (θ == 0.0F) ? 0.0F : ((1.0F - std::cos(θ)) / θ²); - - return Matrix3{{ - 1.0F - c * (w₂² + w₃²), - c * w₁w₂ - s * w₃ᵗ, - c * w₁w₃ + s * w₂ᵗ, - c * w₁w₂ + s * w₃ᵗ, - 1.0F - c * (w₁² + w₃²), - c * w₂w₃ - s * w₁ᵗ, - c * w₁w₃ - s * w₂ᵗ, - c * w₂w₃ + s * w₁ᵗ, - 1.0F - c * (w₁² + w₂²), - }}; -} - -float AltitudeOriginal(float kPa) { - const float x = kPa * 1000.0F; - float sum = 0.0F; - float xn = x; - for (std::size_t i = 1; i < kAtmosphere.size(); ++i) { - sum += kAtmosphere[i] * xn; - xn *= x; - } - return sum + kAtmosphere[0]; -} - -float AltitudeNew(float kPa) { - const float x = kPa * 1000.0F; - float sum = kAtmosphere[kAtmosphere.size() - 1]; - for (std::size_t i = kAtmosphere.size() - 1; i-- > 0;) { - sum = (sum * x) + kAtmosphere[i]; - } - return sum; -} - -Quaternion QuaternionMultiply(const Quaternion& lhs, const Quaternion& rhs) { - return { - (lhs.w * rhs.w) - (lhs.x * rhs.x) - (lhs.y * rhs.y) - (lhs.z * rhs.z), - (lhs.w * rhs.x) + (lhs.x * rhs.w) + (lhs.y * rhs.z) - (lhs.z * rhs.y), - (lhs.w * rhs.y) - (lhs.x * rhs.z) + (lhs.y * rhs.w) + (lhs.z * rhs.x), - (lhs.w * rhs.z) + (lhs.x * rhs.y) - (lhs.y * rhs.x) + (lhs.z * rhs.w), - }; -} - -Quaternion NormalizeQuaternion(const Quaternion& q) { - const float normSq = (q.w * q.w) + (q.x * q.x) + (q.y * q.y) + (q.z * q.z); - if (normSq == 0.0F) { - return {1.0F, 0.0F, 0.0F, 0.0F}; - } - - const float invNorm = 1.0F / std::sqrt(normSq); - return {q.w * invNorm, q.x * invNorm, q.y * invNorm, q.z * invNorm}; -} - -Quaternion QuaternionDeltaFromGyro(float gx, float gy, float gz, float dt) { - const float rx = gx * dt; - const float ry = gy * dt; - const float rz = gz * dt; - const float rotationMagnitudeSq = (rx * rx) + (ry * ry) + (rz * rz); - const float rotationMagnitude = std::sqrt(rotationMagnitudeSq); - - if (rotationMagnitude == 0.0F) { - return {1.0F, 0.0F, 0.0F, 0.0F}; - } - - const float halfAngle = 0.5F * rotationMagnitude; - const float vectorScale = std::sin(halfAngle) / rotationMagnitude; - return {std::cos(halfAngle), rx * vectorScale, ry * vectorScale, rz * vectorScale}; -} - -ArrayVec3 RotateVectorByQuaternion(const Quaternion& q, const ArrayVec3& v) { - const ArrayVec3 u{q.x, q.y, q.z}; - const ArrayVec3 uv = Cross3(u, v); - const ArrayVec3 uuv = Cross3(u, uv); - return { - v[0] + (2.0F * ((q.w * uv[0]) + uuv[0])), - v[1] + (2.0F * ((q.w * uv[1]) + uuv[1])), - v[2] + (2.0F * ((q.w * uv[2]) + uuv[2])), - }; -} - -struct GyroStepResult { - Matrix3 orientation; - bool outOfBounds; -}; - -struct QuaternionGyroStepResult { - Quaternion orientation; - bool outOfBounds; -}; - -GyroStepResult FeedGyroOriginal(Matrix3 orientation, float gx, float gy, float gz, float dt) { - const Matrix3 eAT = ExpGyroOriginal(gx, gy, gz, dt); - orientation = orientation * eAT; - - const auto zInImu = RotateRocketToImu({0.0F, 0.0F, 1.0F}); - const Vector3 initial{{zInImu[0], zInImu[1], zInImu[2]}}; - const Vector3 now = orientation * initial; - const auto rotated = RotateImuToRocket({now.Get(0, 0), now.Get(1, 0), now.Get(2, 0)}); - - (void) rotated; - - const float initialNorm = - std::sqrt((initial.Get(0, 0) * initial.Get(0, 0)) + (initial.Get(1, 0) * initial.Get(1, 0)) + - (initial.Get(2, 0) * initial.Get(2, 0))); - const Vector3 normedNow = initial * (1.0F / initialNorm); - const float dot = (initial.Get(0, 0) * normedNow.Get(0, 0)) + (initial.Get(1, 0) * normedNow.Get(1, 0)) + - (initial.Get(2, 0) * normedNow.Get(2, 0)); - const float offStart = std::acos(dot); - return {orientation, offStart > (30.0F * 3.14159F / 180.0F)}; -} - -QuaternionGyroStepResult FeedGyroNew(Quaternion orientation, float gx, float gy, float gz, float dt) { - constexpr float kCosMaxTilt = 0.8660254037844386F; - static const ArrayVec3 initial = RotateRocketToImu({0.0F, 0.0F, 1.0F}); - - const Quaternion delta = QuaternionDeltaFromGyro(gx, gy, gz, dt); - orientation = NormalizeQuaternion(QuaternionMultiply(orientation, delta)); - - const ArrayVec3 now = RotateVectorByQuaternion(orientation, initial); - const float cosOffStart = ClampUnit(Dot3(initial, now)); - return {orientation, cosOffStart < kCosMaxTilt}; -} - -struct Stats { - double minNs; - double meanNs; -}; - -template -Stats RunBenchmark(const char* name, Fn&& fn, std::size_t iterations, std::size_t rounds) { - std::vector samples; - samples.reserve(rounds); - for (std::size_t round = 0; round < rounds; ++round) { - const auto start = std::chrono::steady_clock::now(); - fn(iterations); - const auto end = std::chrono::steady_clock::now(); - const double elapsedNs = std::chrono::duration(end - start).count(); - samples.push_back(elapsedNs / static_cast(iterations)); - } - - const double minNs = *std::min_element(samples.begin(), samples.end()); - const double meanNs = std::accumulate(samples.begin(), samples.end(), 0.0) / static_cast(samples.size()); - std::printf("%-24s min %.2f ns/op mean %.2f ns/op\n", name, minNs, meanNs); - return {minNs, meanNs}; -} - -} // namespace - -int main() { - constexpr std::size_t kIterations = 1'000'000; - constexpr std::size_t kRounds = 8; - - volatile float sink = 0.0F; - volatile bool sinkBool = false; - - for (std::size_t i = 0; i < 10000; ++i) { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); - sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); - sink += AltitudeOriginal(85.0F + static_cast(i % 50) * 0.1F); - sink += AltitudeNew(85.0F + static_cast(i % 50) * 0.1F); - } - - std::puts("Airbraker math benchmark"); - std::printf("iterations=%zu rounds=%zu\n\n", kIterations, kRounds); - - const auto expOriginal = RunBenchmark( - "expGyro original", - [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - sink += ExpGyroOriginal(gx, gy, gz, 0.01F).Get(0, 0); - } - DoNotOptimize(sink); - }, - kIterations, kRounds); - - const auto expNew = RunBenchmark( - "expGyro new", - [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - sink += ExpGyroNew(gx, gy, gz, 0.01F).Get(0, 0); - } - DoNotOptimize(sink); - }, - kIterations, kRounds); - - const auto altOriginal = RunBenchmark( - "altitude original", - [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) { - sink += AltitudeOriginal(80.0F + static_cast(i % 1000) * 0.02F); - } - DoNotOptimize(sink); - }, - kIterations, kRounds); - - const auto altNew = RunBenchmark( - "altitude new", - [&](std::size_t iterations) { - for (std::size_t i = 0; i < iterations; ++i) { - sink += AltitudeNew(80.0F + static_cast(i % 1000) * 0.02F); - } - DoNotOptimize(sink); - }, - kIterations, kRounds); - - const auto feedOriginal = RunBenchmark( - "feedGyro original", - [&](std::size_t iterations) { - Matrix3 orientation = Matrix3::Identity(); - bool everOut = false; - for (std::size_t i = 0; i < iterations; ++i) { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - const auto result = FeedGyroOriginal(orientation, gx, gy, gz, 0.01F); - orientation = result.orientation; - everOut |= result.outOfBounds; - } - sink += orientation.Get(0, 0); - sinkBool ^= everOut; - DoNotOptimize(sink); - DoNotOptimize(sinkBool); - }, - kIterations, kRounds); - - const auto feedNew = RunBenchmark( - "feedGyro new", - [&](std::size_t iterations) { - Quaternion orientation{1.0F, 0.0F, 0.0F, 0.0F}; - bool everOut = false; - for (std::size_t i = 0; i < iterations; ++i) { - const float gx = 0.02F + static_cast(i % 11) * 0.004F; - const float gy = -0.03F + static_cast(i % 7) * 0.005F; - const float gz = 0.01F + static_cast(i % 13) * 0.003F; - const auto result = FeedGyroNew(orientation, gx, gy, gz, 0.01F); - orientation = result.orientation; - everOut |= result.outOfBounds; - } - sink += orientation.w; - sinkBool ^= everOut; - DoNotOptimize(sink); - DoNotOptimize(sinkBool); - }, - kIterations, kRounds); - - std::puts("\nSpeedups based on best run"); - std::printf("expGyro: %.2fx\n", expOriginal.minNs / expNew.minNs); - std::printf("altitude: %.2fx\n", altOriginal.minNs / altNew.minNs); - std::printf("feedGyro step: %.2fx\n", feedOriginal.minNs / feedNew.minNs); - std::printf("\nignore sinks: %f %d\n", sink, static_cast(sinkBool)); - return 0; -}