From 580a2e4f2739e4c4b405a384e10405a8d03d5794 Mon Sep 17 00:00:00 2001 From: krizzyegg Date: Wed, 19 Nov 2025 19:44:28 -0500 Subject: [PATCH 01/14] Added QMEKF from bolerplate --- QMEKF/Conventions.txt | 19 ++ QMEKF/kfConsts.h | 41 ++++ QMEKF/qmekf.cpp | 483 ++++++++++++++++++++++++++++++++++++++++++ QMEKF/qmekf.h | 226 ++++++++++++++++++++ 4 files changed, 769 insertions(+) create mode 100644 QMEKF/Conventions.txt create mode 100644 QMEKF/kfConsts.h create mode 100644 QMEKF/qmekf.cpp create mode 100644 QMEKF/qmekf.h diff --git a/QMEKF/Conventions.txt b/QMEKF/Conventions.txt new file mode 100644 index 0000000..eea600a --- /dev/null +++ b/QMEKF/Conventions.txt @@ -0,0 +1,19 @@ +Notes: + + +I change magData variable to imuData because it contains all the IMU data, not just magData +Also please change the accel to return the m/s^2, not g's +I change take in int variable state to onLoop, not the boolean: + 0 = preLaunch + 1 = high thrust + 2 = coast + 3 = falling down + + + +Frequencies variable in format: + priori step + accel upate + mag update + gps update + baro update \ No newline at end of file diff --git a/QMEKF/kfConsts.h b/QMEKF/kfConsts.h new file mode 100644 index 0000000..a9e7c98 --- /dev/null +++ b/QMEKF/kfConsts.h @@ -0,0 +1,41 @@ +#pragma once + +#include +constexpr static float g = 9.80665; // [m/s/s] Earth's Grav Accel + +constexpr float square(float a) { return a * a; } + +namespace asm330_const { +constexpr float accelXY_var = square(0.0020f); // [g] +constexpr float accelZ_var = square(0.0014f); // [g] +constexpr float accelXY_VRW = 0.0003f; // [g/sqrt(Hz)] +constexpr float accelZ_VRW = 0.0002f; // [g/sqrt(Hz)] +constexpr float gyroX_var = square(0.0947f); // [deg/s] +constexpr float gyroY_var = square(0.1474f); // [deg/s] +constexpr float gyroZ_var = square(0.2144f); // [deg/s] +constexpr float gyro_VRW = 0.0241f; // [deg/s/sqrt(Hz)] +}; // namespace asm330_const + +namespace lps22_const { +constexpr float baro_var = 0.0f; +}; + +namespace icm20948_const { +constexpr float accelXY_var = square(0.0383f); // [g] +constexpr float accelZ_var = square(0.0626f); // [g] +constexpr float accelXY_VRW = 0.0062f; // [g/sqrt(Hz)] +constexpr float accelZ_VRW = 0.0099f; // [g/sqrt(Hz)] +// constexpr float gyroXYZ_var = square(0.0051); // [rad/s] +constexpr float gyroXYZ_var = square(5e-4); // [rad/s] +constexpr float gyro_VRW = 8.33e-4f; // [rad/s/sqrt(Hz)] +constexpr float magXYZ_var = square(0.7263f); // [uT] +constexpr float quatVar = 0.3; // Idk Guess + + +}; // namespace icm20948_const + + +namespace Max10S_const { + constexpr float gpsXYZ_var = square(2.0); // [m] idk guess + constexpr float baro_var = square(7.5); // [P] idk guess +} diff --git a/QMEKF/qmekf.cpp b/QMEKF/qmekf.cpp new file mode 100644 index 0000000..2735e67 --- /dev/null +++ b/QMEKF/qmekf.cpp @@ -0,0 +1,483 @@ +#include "QMEKF.h" + +#include "BasicLinearAlgebra.h" + +#include +#include +#include +#include "boilerplate/Logging/Loggable.h" +#include "boilerplate/Sensors/Impl/ICM20948.h" +#include "boilerplate/TimedPointer/TimedPointer.h" + + +StateEstimator::StateEstimator(const TimedPointer IMUData, + const TimedPointer gpsData, + float dt) : IMUData(IMUData), gpsData(gpsData) { + P.Fill(0.0f); + for(uint8_t idx : QMEKFInds::quat) { + P(idx, idx) = 1e-8; + } + for (uint8_t idx : QMEKFInds::vel) { + P(idx, idx) = 1e-8; + } + for (uint8_t idx : QMEKFInds::pos) { + P(idx, idx) = 1e-8; + } + for(uint8_t idx : QMEKFInds::gyroBias) { + P(idx, idx) = powf(icm20948_const::gyro_VRW, 2.0f); + } + for(uint8_t idx: QMEKFInds::accelBias) { + P(idx, idx) = powf(icm20948_const::accelXY_VRW, 2.0f); + } + for(uint8_t idx : QMEKFInds::magBias) { + // P(idx, idx) = icm20948_const.magXYZ_var; + P(idx, idx) = powf(0.1f, 2); + } + for (uint8_t idx : QMEKFInds::baroBias) { + P(idx, idx) = powf(0.1f, 2); + } + + P_min = P; + + + //ToDo: Look at this alter, big ass matrix or integration + Q.Fill(0.0f); + for(uint8_t idx : QMEKFInds::quat) { + Q(idx, idx) = 1e-8; + } + for(uint8_t idx : QMEKFInds::vel) { + Q(idx, idx) = 1e-8; + } + for(uint8_t idx : QMEKFInds::pos) { + Q(idx, idx) = 1e-8; + } + for(uint8_t idx : QMEKFInds::gyroBias) { + Q(idx, idx) = powf(0.1f,2); + } + for(uint8_t idx : QMEKFInds::accelBias) { + Q(idx, idx) = powf(0.00f,2); + } + for(uint8_t idx : QMEKFInds::magBias) { + Q(idx, idx) = powf(0.1f, 2); + } + for(uint8_t idx : QMEKFInds::baroBias) { + Q(idx, idx) = powf(0.1f, 2); + } + +#ifdef DBG + Serial.println("<----- Process Noise ----->"); + for (int i = 0; i < Q.Rows; i++) { + for (int j = 0; j < Q.Cols; j++) { + Serial.print(String(Q(i,j)) + "\t"); + } + Serial.println(""); + } + + Serial.println("<----- Initial Error Cov ----->"); + for (int i = 0; i < P.Rows; i++) { + for (int j = 0; j < P.Cols; j++) { + Serial.print(String(P(i,j)) + "\t"); + } + Serial.println(""); + } +#endif + +} +void StateEstimator::init(BLA::Matrix<3, 1> LLA){ + // Accelerometer + BLA::Matrix<3,1> a_b = { + IMUData->accelX, + IMUData->accelY, + IMUData->accelZ + }; + a_b = a_b / BLA::Norm(a_b); + + // Flip gravity direction: ensure z points DOWN in body frame + float ax = a_b(0), ay = a_b(1), az = a_b(2); + + // Compute roll (phi) and pitch (theta) assuming NED frame + float roll = atan2(-ay, -az); // Flip signs to match z-down NED + float pitch = atan2(ax, sqrt(ay*ay + az*az)); + + // Magnetometer + BLA::Matrix<3,1> m_b = { + IMUData->magX, + IMUData->magY, + IMUData->magZ + }; + + // Tilt compensation + float mx = m_b(0), my = m_b(1), mz = m_b(2); + + float mx2 = mx * cos(pitch) + mz * sin(pitch); + float my2 = mx * sin(roll) * sin(pitch) + my * cos(roll) - mz * sin(roll) * cos(pitch); + + float yaw = atan2(-my2, mx2); // Heading angle (NED convention) + + // Convert roll/pitch/yaw to quaternion (ZYX convention) + float cy = cos(yaw * 0.5f); + float sy = sin(yaw * 0.5f); + float cp = cos(pitch * 0.5f); + float sp = sin(pitch * 0.5f); + float cr = cos(roll * 0.5f); + float sr = sin(roll * 0.5f); + + BLA::Matrix<4,1> q0 = { + cr * cp * cy + sr * sp * sy, // w + sr * cp * cy - cr * sp * sy, // x + cr * sp * cy + sr * cp * sy, // y + cr * cp * sy - sr * sp * cy // z + }; + + this->x = {q0(0), q0(1), q0(2), q0(3), + 0, 0, 0, // velocity + 0, 0, 0, // position + 0, 0, 0, // gyro bias + 0, 0, 0, // accel bias + 0, 0, 0, // mag bias + 0}; // baro bias + + this->x_min = x; + + // Set launch site LLA/ECEF + launch_ecef = QuaternionUtils::lla2ecef(LLA); + launch_lla = LLA; + R_ET = QuaternionUtils::dcm_ned2ecef(launch_lla(0), launch_lla(1)); + + lastTimes = {millis(), millis(), millis(), millis(), millis()}; +} + + +BLA::Matrix<20,1> StateEstimator::onLoop(int state) { + // Read data from sensors and convert values + + float gyrX = ICMData->gyrX; + float gyrY = ICMData->gyrY; + float gyrZ = ICMData->gyrZ; + + float aclX = ICMData->accelX; + float aclY = ICMData->accelY; + float aclZ = ICMData->accelZ; + + float magX = ICMData->magX; + float magY = ICMData->magY; + float magZ = ICMData->magZ; + + + // TODO get in the GPS data and baro data from somewhere + BLA::Matrix<3,1> gyro = {gyrX, gyrY, gyrZ}; // [rad/s] + BLA::Matrix<3,1> accel = {aclX, aclY, aclZ}; // [m/s^s] + BLA::Matrix<3,1> mag = {magX, magY, magZ}; // [uT] + BLA::Matrix<3,1> gpsData = {gpsData(0), gpsData(1), gpsData(2)}; + // BLA::Matrix<1,1> baro = {baroZ}; + + // Remove biases from each measurement + BLA::Matrix<3,1> unbiased_gyro = {gyro(0) - x(QMEKFInds::gb_x), gyro(1) - x(QMEKFInds::gb_y), gyro(2) - x(QMEKFInds::gb_z)}; + BLA::Matrix<3,1> unbiased_accel = {accel(0) - x(QMEKFInds::ab_x), accel(1) - x(QMEKFInds::ab_y), accel(2) - x(QMEKFInds::ab_z)}; + BLA::Matrix<3,1> unbiased_mag = {mag(0) - x(QMEKFInds::mb_x), mag(1) - x(QMEKFInds::mb_y), mag(2) - x(QMEKFInds::mb_z)}; + // BLA::Matrix<1,1> unbiased_baro = {baro(0) - x(20)}; + + + float time = millis(); + bool run_priori = time - lastTimes(0) >= frequencies(0); + bool run_accel_update = time - lastTimes(1) >= frequencies(1); + bool run_mag_update = time - lastTimes(2) >= frequencies(2); + bool run_gps_update = time - lastTimes(3) >= frequencies(3); + // bool run_baro_update = time - lastTimeBaro(4) >= frequencies(4); + + + if(run_priori) { + // TODO eventually implement RK4 here, but I don't understand it yet + float lastAttUpdate = BLA::max(lastTimes(0), lastTimes(1), lastTimes(2)); // Maximum of the lastTimes(0, 1, 2) + float lastPVUpdate = BLA::max(lastTimes(0), lastTimes(3), lastTimes(4)); // Maximum of the lastTimes(0, 3, 4) + float dt_att = millis() - lastAttUpdate; + float dt_pv = millis() - lastPVUpdate; + + x = fastIMUProp(gyro, accel, dt_att, dt_pv); + + + lastTimes(0) = millis(); + } + + if (run_accel_update || run_mag_update || run_gps_update) { + dt = millis() - BLA::max(lastTimes(1), lastTimes(2), lastTimes(3), lastTimes(4)); + P = predictionFunction(P, gyro, accel, dt); + } + + if (run_accel_update) { + runAccelUpdate(); + lastTimes(1) = millis(); + } + + if (run_mag_update) { + runMagUpdate(); + lastTimes(2) = millis(); + } + + if (run_gps_update) { + runGPSUpdate(QuaternionUtils::lla2ecef(gpsData)); + lastTimes(3) = millis(); + } + + // if (run_baro_update) { + // run_baro_update(); + // lastTimes(4) = millis(); + // } + + //Update sensor readings + gyro_prev = unbiased_gyro; + accel_prev = unbiased_accel; + mag_prev = unbiased_mag; + gps_prev = gpsData; + // baro_prev = unbiased_baro; + + return x; +} + +BLA::Matrix<20,1> StateEstimator::fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt) { + + // TODO change to from world model when go to ECEF + BLA::Matrix<3,1> gyro_int = {gyro(0)*att_dt, gyro(1)*att_dt, gyro(2)*att_dt}; + BLA::Matrix<3,1> rotVecNorm = BLA::Norm(gyro_int); + BLA::Matrix<3,1> axis = {gyro_int(0) / rotVecNorm, gyro_int(1) / rotVecNorm, gyro_int(2) / rotVecNorm}; + BLA::Matrix<3,1> dq = + { + cos(rotVecNorm(0)/2.0f), + axis(0) * sinf(rotVecNorm(0)/2.0f), + axis(1) * sinf(rotVecNorm(0)/2.0f), + axis(2) * sinf(rotVecNorm(0)/2.0f), + }; + BLA::Matrix<4,1> q = + { + x(QMEKFInds::q_w), + x(QMEKFInds::q_x), + x(QMEKFInds::q_y), + x(QMEKFInds::q_z), + }; + q = QuaternionUtils::quatMultiply(q, dq); + BLA::Matrix<4,1> qNorm = {q(0) / BLA::Norm(q), q(1) / BLA::Norm(q), q(2) / BLA::Norm(q), q(3) / BLA::Norm(q)}; + + + + BLA::Matrix<3,1> v_dot = QuaternionUtils::quatToRot(q) * accel + g_i; + BLA::Matrix<3,1> old_v = { + x(QMEKFInds::v_x), x(QMEKFInds::v_y), x(QMEKFInds::v_z) + } + BLA::Matrix<3,1> old_p = { + x(QMEKFInds::p_x), x(QMEKFInds::p_y), x(QMEKFInds::p_z) + } + v = old_v + v_dot * pv_dt; + p = old_p + v * pv_dt; + + x(QMEKFInds::v_x) = v(0); + x(QMEKFInds::v_y) = v(1); + x(QMEKFInds::v_z) = v(2); + x(QMEKFInds::p_x) = p(0); + x(QMEKFInds::p_y) = p(1); + x(QMEKFInds::p_z) = p(2); +} + +BLA::Matrix<19, 1> StateEstimator::predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt) { + BLA::Matrix<3,3> gyroSkew = QuaternionUtils::skewSymmetric(gyroVec); + BLA::Matrix<3,3> accelSkew = QuaternionUtils::skewSymmetric(accelVec); + + BLA::Matrix<4, 1> q = + { + x(QMEKFInds::q_w), + x(QMEKFInds::q_x), + x(QMEKFInds::q_y), + x(QMEKFInds::q_z) + }; + + BLA::Matrix<3, 3> rotMatrix = QuaternionUtils::quatToRot(q); + + BLA::Matrix<19, 19> F; + F = F.Fill(0); + + //Row 1 - 3 + F.subMatrix<3, 3>(0, QMEKFInds::q_w) = -1 * gyroSkew; + F.subMatrix<3, 3>(0, QMEKFInds::gb_x) = -1 * I_3; + + //Row 4 - 6 + F.subMatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::q_w) = -1 * rotMatrix * accelSkew; + F.subMatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::ab_x) = -1 * rotMatrix; + + //Row 7 - 9 + F.subMatrix<3, 3>(QMEKDInds::P_x -1, 3) = I_3; + + BLA::Matrix<19, 19> phi; + phi = phi.Fill(0); + + phi = I_19 + (F * dt) + (0.5 * F * F * pow(dt, 2)); + + BLA::Matrix<19, 19> phi_t = ~phi; + + BLA::Matrix<19, 19> Q_d; + Q_d = Q_d.Fill(0); + + BLA::Matrix<3, 3> gyro_var_diag; + gyro_var_diag = gyro_var_diag.Fill(0); + gyro_var_diag(0, 0) = QMEKFInds::gyro_var; + gyro_var_diag(1, 1) = QMEKFInds::gyro_var; + gryo_var_diag(2, 2) = QMEKFInds::gyro_var; + + BLA::Matrix<3, 3> gyro_bias_var_diag; + gyro_bias_var_diag = gyro_bias_var_diag.Fill(0); + gyro_bias_var_diag(0, 0) = QMEKFInds::gyro_bias_var; + gyro_bias_var_diag(1, 1) = QMEKFInds::gyro_bias_var; + gyro_bias_var_diag(2, 2) = QMEKFInds::gyro_bias_var; + + BLA::Matrix<3, 3> accel_bias_var_diag; + accel_bias_var_diag = accel_bias_var_diag.Fill(0); + accel_bias_var_diag(0, 0) = QMEKFInds::accel_bias_var; + accel_bias_var_diag(1, 1) = QMEKFInds::accel_bias_var; + accel_bias_var_diag(2, 2) = QMEKFInds::accel_bias_var; + + Q_d.subMatrix<3, 3>(QMEKFInds::q_w, QMEKFInds::q_w) = (gyro_var_diag * dt) + (gyro_bias_var_diag * (pow(dt, 3) / 10)); + Q_d.subMatrix<3, 3>(QMEKFInds::q_w, 9) = -1 * gyro_bias_var_diag * (pow(dt, 2) / 2); + + Q_d.subMatrix<3 ,3>(3, 3) = QMEKF::Inds::R_Grav * dt + aaccel_bias_var_diag * (pow(dt, 3) / 3); + Q_d.subMatrix<3, 3>(3, 6) = accel_bias_var_diag * (pow(dt ,4) / 8.0) + QMEKFInds::R_grav * (pow(dt, 2) / 2.0); + Q_d.subMatrix<3, 3>(3, 10) = -1.0 * accel_bias_var_diag * (pow(dt, 2) / 2.0); + + Q_d.subMatrix<3, 3>(6, 3) = QMEKFInds::R_grav * (pow(dt, 2) / 2) + accel_bias_var_diag * (pow(dt, 4) / 8.0); + Q_d.subMatrix<3, 3>(6, 6) = QMEKFInds::R_grav * (pow(dt, 3) / 3.0) + accel_bias_var_diag * (pow(dt, 5) / 20.0); + Q_d.subMatrix<3, 3>(6, 10) = -1.0 * accel_bias_var_diag * (pow(dt, 3) / 6.0); + + Q_d.subMatix<3, 3>(9, 0) = -1.0 * gyro_bias_var_diag * (pow(dt, 2) / 2.0); + Q_d.subMatrix<3, 3>(9, 9) = gyro_bias_var_diag * (pow(dt, 2) / 2.0); + + Q_d.subMatrix<3, 3>(12, 3) = -1.0 * accel_bias_var_diag * (pow(dt, 2) / 2.0); + Q_d.subMatrix<3, 3>(12, 6) = -1.0 * accel_bias_var_diag * (pow(dt, 2) / 2.0); + Q_d.subMatrix<3, 3>(12, 12) = accel_bias_var_diag * dt; + + Q_d(15, 15) = mag_bias_var * dt; + + Q_d(18, 18) = baro_bias_var * dt; + + BLA::Matrix<19, 19> P; + + P = phi * P_ * phi_t + Q_D; + +} + +BLA::Matrix<20, 1> StateEstimator::runAccelUpdate(BLA::Matrix<3,1> accel_meas) +{ + BLA::Matrix<4,1> q = + { + x(QMEKFInds::q_w), + x(QMEKFInds::q_x), + x(QMEKFInds::q_y), + x(QMEKFInds::q_z), + }; + + BLA::Matrix<3, 20> H_accel; + H_accel = H_accel.Fill(0); + H_accel.subMatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * (-1.0 * QMEKFInds::normal_i;)); + H_accel.subMatrix<3, 3>(0, QMEKFInds::ab_x - 1) = -1.0 * QuaternionUtils::quat2DCM(q); + + h_accel = QuaternionUtils::quat2DCM(q) * QMEKFInds::normal_i; + + BLA::Matrix<3, 3> R = subMatrix(R_all) + + EKFCalcErrorInject(x, P, H_accel, h_accel, R); + +} + +BLA::Matrix<20, 1> StateEstimator::runMagUpdate(BLA::Matrix<3, 1> mag_meas) { + // TODO input igrm model somehow figure out + BLA::Matrix<4, 1> q = + { + x(QMEKFInds::q_w), + x(QMEKFInds::q_x), + x(QMEKFInds::q_y), + x(QMEKFInds::q_z) + }; + + BLA::Matrix<3, 20> H_mag; + H_mag = H_mag.Fill(0); + H_mag.subMatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * igrm_model); + H_mag.subMatrix<3, 3>(0, QMEKFInds::gb_x) = I_3; + + h_mag = QuaternionUtils::quat2DCM(q) * igrm_model; + + BLA::Matrix<3, 3> R = subMatrix(R_all); // IDK something + + EKFCalcErrorInject(x, P, mag_meas, H_mag, h_mag, R); + +} + +BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<3, 1> gps_meas_ecef) { + BLA::Matrix<3, 1> pos_ned = QuaternionUtils::ecef2ned(gps_meas_ecef, launch_ecef, R_ET); + + BLA::Matrix<3, 20> H_gps; + H_gps = H_gps.Fill(0); + H_gps.SubMatrix<3, 3>(0, QMEKFInds::gps_x) = I_3; + + BLA::Matrix<3, 1> h_gps = { + x(QMEKFInds::gps_x), + x(QMEKFInds::gps_y), + x(QMEKFInds::gps_z), + }; + + BLA::Matrix<3, 3> R = subMatrix(R_all); // not right, fix + + EKFCalcErrorInject(x, P, pos_ned, H_gps, h_gps, R); + +} + +BLA::Matrix<20, 1> StateEstimator::EKFCalcErrorInject(BLA::Matrix<20, 1> oldState, BLA::Matrix<19, 19> oldP, BLA::Matrix sens_reading, BLA::Matrix H_matrix, BLA::Matrix h, BLA::Matrix R) { + residual = sens - h_matrix; + + S = H * oldP * ~H + R; + K = (oldP * ~H) * BLA::Inverse(S); + BLA::Matrix<19, 1> postErrorState = K * residual; + + // Inject error angles into quat + BLA::Matrix<3, 1> rotVec = 1.0 * posterioriErrorState(1:3); + float rotVecNorm = BLA::Norm(rotVec); + BLA::Matrix<3,1> axis = rotVec / rotVecNorm; + BLA::Matrix<3,1> dq = + { + cos(rotVecNorm(0)/2.0f) + axis(0) * sinf(rotVecNorm(0)/2.0f), + axis(1) * sinf(rotVecNorm(0)/2.0f), + axis(2) * sinf(rotVecNorm(0)/2.0f), + }; + BLA::Matrix q = QuaternionUtils::quatMultiply(x(1:4), dq); + + // Set quats + x(0) = q(0); + x(1) = q(1); + x(2) = q(2); + x(3) = q(3); + + // Set velocity + x(4) = x(4) + postErrorState(3); + x(5) = x(5) + postErrorState(4); + x(6) = x(6) + postErrorState(5); + + // Set position + x(7) = x(7) + postErrorState(6); + x(8) = x(8) + postErrorState(7); + x(9) = x(9) + postErrorState(8); + + // Set gyro bias + x(10) = x(10) + postErrorState(9); + x(11) = x(11) + postErrorState(10); + x(12) = x(12) + postErrorState(11); + + // Set accel bias + x(13) = x(13) + postErrorState(12); + x(14) = x(14) + postErrorState(13); + x(15) = x(15) + postErrorState()4; + + // Set mag bias + x(16) = x(16) + postErrorState(15); + x(17) = x(17) + postErrorState(16); + x(18) = x(18) + postErrorState(17); + + // Set baro bias + x(19) = x(19) + postErrorState(18); +} + diff --git a/QMEKF/qmekf.h b/QMEKF/qmekf.h new file mode 100644 index 0000000..aad5a5d --- /dev/null +++ b/QMEKF/qmekf.h @@ -0,0 +1,226 @@ +#pragma once + +#include "BasicLinearAlgebra.h" +#include +#include "boilerplate/Logging/Loggable.h" +#include "boilerplate/Sensors/Impl/ICM20948.h" +#include "boilerplate/Sensors/Impl/LPS22.h" +#include "boilerplate/Sensors/Impl/MAX10S.h" +#include "boilerplate/TimedPointer/TimedPointer.h" +#include "kfConsts.h" +#include + +/** + * @name QMEKfInds + * @brief Struct holding the indices of the total QMEKF + */ +namespace QMEKFInds { +constexpr uint8_t q_w = 0; +constexpr uint8_t q_x = 1; +constexpr uint8_t q_y = 2; +constexpr uint8_t q_z = 3; +constexpr std::array quat = {q_w, q_x, q_y, q_z}; + +constexpr uint8_t v_x = 4; +constexpr uint8_t v_y = 5; +constexpr uint8_t v_z = 6; +constexpr std::array vel = {v_x, v_y, v_z}; + +constexpr uint8_t p_x = 7; +constexpr uint8_t p_y = 8; +constexpr uint8_t p_z = 9; +constexpr std::array pos = {p_x, p_y, p_z}; + +constexpr uint8_t gb_x = 10; +constexpr uint8_t gb_y = 11; +constexpr uint8_t gb_z = 12; +constexpr std::array gyroBias = {gb_x, gb_y, gb_z}; + +constexpr uint8_t ab_x = 13; +constexpr uint8_t ab_y = 14; +constexpr uint8_t ab_z = 15; +constexpr std::array accelBias = {ab_x, ab_y, ab_z}; + +constexpr uint8_t mb_x = 16; +constexpr uint8_t mb_y = 17; +constexpr uint8_t mb_z = 18; +constexpr std::array magBias = {mb_x, mb_y, mb_z}; + +constexpr uint8_t bb_z = 19; +constexpr std::array baroBias = {bb_z}; + +constexpr float gyro_var = 0.0051; +constexpr float gyro_bias_var = pow(pow(4.9, -5) * 9.8, 2); + +constexpr float accel_var = pow(4.9 * pow(10, -5) * 9.9, 2); +constexpr float accel_bias_var = pow(0.00098 * 9.8, 2); + +constexpr float mag_bias_var = 25; +constexpr float baro_bias_var = pow(7.5, 2); + + +}; // namespace QMEKFInds + +#define QMEKF_LOG_DESC(X) \ + X(0, "w", p.print(state(QMEKFInds::q_w), 4)) \ + X(1, "i", p.print(state(QMEKFInds::q_x), 4)) \ + X(2, "j", p.print(state(QMEKFInds::q_y), 4)) \ + X(3, "k", p.print(state(QMEKFInds::q_z), 4)) \ + X(4, "v_x", p.print(state(QMEKFInds::v_x), 4)) \ + X(5, "v_y", p.print(state(QMEKFInds::v_y), 4)) \ + X(6, "v_z", p.print(state(QMEKFInds::v_z), 4)) \ + X(7, "p_x", p.print(state(QMEKFInds::p_x), 4)) \ + X(8, "p_y", p.print(state(QMEKFInds::p_y), 4)) \ + X(9, "p_z", p.print(state(QMEKFInds::p_z), 4)) \ + + +class QMEKFLogger : public Loggable { + public: + QMEKFLogger() : Loggable(NUM_FIELDS(QMEKF_LOG_DESC)) {} + + void newState(BLA::Matrix<20, 1> s) { + initialized = true; + state = s; + updatedAt = millis(); + } + + const BLA::Matrix<20, 1>& getState() { return state; } + + private: + BLA::Matrix<20, 1> state; + uint32_t updatedAt = 0; + bool initialized = false; + + MAKE_LOGGABLE(QMEKF_LOG_DESC) + + uint32_t dataUpdatedAt() override { + if (!initialized) return 0; // Data will not be loggged + return updatedAt; + } +}; + + + + +/** + * @name QMEKFStateEstimator + * @author QMEKF team + * @brief Attitude and Position/Velocity estimation. See matlab simulation for details + */ +class StateEstimator { + + public: + StateEstimator(const TimedPointer, const TimedPointer, float dt); + + /** + * @name init + * @author @frostydev99 + * @param x_0 - Initial State + * @param dt - Discrete time step + */ + void init(BLA::Matrix<3, 1> LLA); + + /** + * @name onLoop + * @author @frostydev99 + * @brief Run Every Loop + * @paragraph This method should run every loop of the expected prediction + * update rate given by dt + */ + BLA::Matrix<20, 1> onLoop(int state); + + private: + const TimedPointer IMUData; + const TimedPointer gpsData; + + // Prediction Functions + // BLA::Matrix<20, 1> predictionFunction(BLA::Matrix<20, 1> x, + // BLA::Matrix<3, 1> gyro, + // BLA::Matrix<3, 1> accel); + // BLA::Matrix<19, 19> predictionJacobian(BLA::Matrix<20, 1> x, + // BLA::Matrix<3, 1> gyro, + // BLA::Matrix<3, 1> accel); + + BLA::Matrix<20,1> fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt); + BLA::Matrix<19, 1> predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt); + + // Update Functions + void runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> a_b); + + void runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> m_b); + + void runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps); + + // void runBaroUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<1, 1> baro); + + void EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 20> H_matrix, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R); + + // State Vector Allocation + BLA::Matrix<20, 1> x_min; + + BLA::Matrix<20, 1> x; + + // Error Covariance Allocation + BLA::Matrix<19, 19> P; + + BLA::Matrix<19, 19> P_min; + + // Process Noise Covariance Allocation + BLA::Matrix<19, 19> Q; + + // Identity Matrices + BLA::Matrix<20, 20> I_20 = BLA::Eye<20, 20>(); + BLA::Matrix<19, 19> I_19 = BLA::Eye<19, 19>(); + BLA::Matrix<3, 3> I_3 = BLA::Eye<3, 3>(); + + + BLA::Matrix<10, 1> R_all = { + pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), + pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), + pow(sqrt(asm330_const::accelZ_var) * 9.8, 2), + icm20948_const::magXYZ_var, + icm20948_const::magXYZ_var, + icm20948_const::magXYZ_var, + Max10S_const::gpsXYZ_var, + Max10S_const::gpsXYZ_var, + Max10S_const::gpsXYZ_var, + Max10S_const::baro_var + }; + + + BLA::Matrix<5, 1> lastTimes; + + // In the format defined above, how often to run stuff + BLA::Matrix<5, 1> frequencies = { + 100, + 300, + 400, + 2000, + 1000}; + + BLA::Matrix<3,1> gyro_prev; + BLA::Matrix<3,1> accel_prev; + BLA::Matrix<3,1> mag_prev; + BLA::Matrix<3,1> gps_prev; + + BLA::Matrix<3, 1> normal_i = {0, 0, -9.8037}; // [m/s^2] + BLA::Matrix<3, 1> g_i = {0, 0, 9.8037}; // [m/s^2] + BLA::Matrix<3, 1> m_i = {18.659605, -4.540227, 49.09786}; // [uT] Kids rocket params + BLA::Matrix<3, 1> launch_ecef = {1311800, -4337300, 4473600}; // [m] // asuming 0 above surface + BLA::Matrix<3, 1> launch_lla = {44.825070, -73.171726, 0}; // [whatever tf units are in (deg, deg, m)] + BLA::Matrix<2, 1> launch_ll = {launch_lla(0), launch_lla(1)}; + BLA::Matrix<3, 3> R_ET = QuaternionUtils::dcm_ned2ecef(launch_ll); + +template +BLA::Matrix extractSub(const BLA::Matrix &x, + const std::array &inds) { + BLA::Matrix sub; + for (int i = 0; i < M; i++) { + sub(i) = x(inds[i]); + } + return sub; +} + +BLA::Matrix<3, 3> quat2rot(const BLA::Matrix<4, 1> &q); + +}; From 287965a887de907766ca38d76631306c90501726 Mon Sep 17 00:00:00 2001 From: krizzyegg Date: Wed, 19 Nov 2025 20:28:27 -0500 Subject: [PATCH 02/14] Packages Added --- .vscode/c_cpp_properties.json | 25 ++++ QMEKF/qmekf.cpp | 233 +++++++++++++++++++--------------- QMEKF/qmekf.h | 34 +++-- 3 files changed, 178 insertions(+), 114 deletions(-) create mode 100644 .vscode/c_cpp_properties.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..a75e91e --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,25 @@ +{ + "configurations": [ + { + "name": "Win32", + "includePath": [ + "${workspaceFolder}/**", + "C:/Users/krizz/OneDrive/Documents/PlatformIO/Projects/QMEKF-Script/.pio/libdeps/disco_h747xi/BasicLinearAlgebra", + "C:/Users/krizz/OneDrive/Documents/HPRC", + "C:/Users/krizz/.platformio/packages/framework-arduinoststm32/cores/arduino", + "C:/Users/krizz/.platformio/packages/framework-arduinoststm32/libraries/SrcWrapper/inc", + "C:/Users/krizz/.platformio/packages/framework-arduinoststm32/libraries/SrcWrapper/inc/LL" + ], + "defines": [ + "_DEBUG", + "UNICODE", + "_UNICODE" + ], + "compilerPath": "C:\\msys64\\ucrt64\\bin\\gcc.exe", + "cStandard": "c17", + "cppStandard": "gnu++17", + "intelliSenseMode": "windows-gcc-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/QMEKF/qmekf.cpp b/QMEKF/qmekf.cpp index 2735e67..4b6c1bd 100644 --- a/QMEKF/qmekf.cpp +++ b/QMEKF/qmekf.cpp @@ -1,14 +1,8 @@ -#include "QMEKF.h" +#include "ekf-implementation/QMEKF/qmekf.h" #include "BasicLinearAlgebra.h" #include -#include -#include -#include "boilerplate/Logging/Loggable.h" -#include "boilerplate/Sensors/Impl/ICM20948.h" -#include "boilerplate/TimedPointer/TimedPointer.h" - StateEstimator::StateEstimator(const TimedPointer IMUData, const TimedPointer gpsData, @@ -55,7 +49,7 @@ StateEstimator::StateEstimator(const TimedPointer IMUData, Q(idx, idx) = powf(0.1f,2); } for(uint8_t idx : QMEKFInds::accelBias) { - Q(idx, idx) = powf(0.00f,2); + Q(idx, idx) = powf(0.01f,2); } for(uint8_t idx : QMEKFInds::magBias) { Q(idx, idx) = powf(0.1f, 2); @@ -142,7 +136,8 @@ void StateEstimator::init(BLA::Matrix<3, 1> LLA){ // Set launch site LLA/ECEF launch_ecef = QuaternionUtils::lla2ecef(LLA); launch_lla = LLA; - R_ET = QuaternionUtils::dcm_ned2ecef(launch_lla(0), launch_lla(1)); + BLA::Matrix<2, 1> ll = {launch_lla(0), launch_lla(1)}; + R_ET = QuaternionUtils::dcm_ned2ecef(ll); lastTimes = {millis(), millis(), millis(), millis(), millis()}; } @@ -151,17 +146,19 @@ void StateEstimator::init(BLA::Matrix<3, 1> LLA){ BLA::Matrix<20,1> StateEstimator::onLoop(int state) { // Read data from sensors and convert values - float gyrX = ICMData->gyrX; - float gyrY = ICMData->gyrY; - float gyrZ = ICMData->gyrZ; + float dt; + + float gyrX = IMUData->gyrX; + float gyrY = IMUData->gyrY; + float gyrZ = IMUData->gyrZ; - float aclX = ICMData->accelX; - float aclY = ICMData->accelY; - float aclZ = ICMData->accelZ; + float aclX = IMUData->accelX; + float aclY = IMUData->accelY; + float aclZ = IMUData->accelZ; - float magX = ICMData->magX; - float magY = ICMData->magY; - float magZ = ICMData->magZ; + float magX = IMUData->magX; + float magY = IMUData->magY; + float magZ = IMUData->magZ; // TODO get in the GPS data and baro data from somewhere @@ -188,8 +185,8 @@ BLA::Matrix<20,1> StateEstimator::onLoop(int state) { if(run_priori) { // TODO eventually implement RK4 here, but I don't understand it yet - float lastAttUpdate = BLA::max(lastTimes(0), lastTimes(1), lastTimes(2)); // Maximum of the lastTimes(0, 1, 2) - float lastPVUpdate = BLA::max(lastTimes(0), lastTimes(3), lastTimes(4)); // Maximum of the lastTimes(0, 3, 4) + float lastAttUpdate = max(max(lastTimes(0), lastTimes(1)), lastTimes(2)); // Maximum of the lastTimes(0, 1, 2) + float lastPVUpdate = max(max(lastTimes(0), lastTimes(3)), lastTimes(4)); // Maximum of the lastTimes(0, 3, 4) float dt_att = millis() - lastAttUpdate; float dt_pv = millis() - lastPVUpdate; @@ -200,22 +197,23 @@ BLA::Matrix<20,1> StateEstimator::onLoop(int state) { } if (run_accel_update || run_mag_update || run_gps_update) { - dt = millis() - BLA::max(lastTimes(1), lastTimes(2), lastTimes(3), lastTimes(4)); + dt = millis() - max(max(lastTimes(0), lastTimes(1)), max(lastTimes(2), lastTimes(3)));; P = predictionFunction(P, gyro, accel, dt); } if (run_accel_update) { - runAccelUpdate(); + runAccelUpdate(x, accel); lastTimes(1) = millis(); } if (run_mag_update) { - runMagUpdate(); + runMagUpdate(x, mag); lastTimes(2) = millis(); } if (run_gps_update) { - runGPSUpdate(QuaternionUtils::lla2ecef(gpsData)); + BLA::Matrix<3,1> gps_ecef = QuaternionUtils::lla2ecef(gpsData); + runGPSUpdate(x, gps_ecef); lastTimes(3) = millis(); } @@ -235,17 +233,20 @@ BLA::Matrix<20,1> StateEstimator::onLoop(int state) { } BLA::Matrix<20,1> StateEstimator::fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt) { - + + BLA::Matrix<3, 1> v; + BLA::Matrix<3, 1> p; + // TODO change to from world model when go to ECEF BLA::Matrix<3,1> gyro_int = {gyro(0)*att_dt, gyro(1)*att_dt, gyro(2)*att_dt}; - BLA::Matrix<3,1> rotVecNorm = BLA::Norm(gyro_int); - BLA::Matrix<3,1> axis = {gyro_int(0) / rotVecNorm, gyro_int(1) / rotVecNorm, gyro_int(2) / rotVecNorm}; - BLA::Matrix<3,1> dq = + float rotVecNorm = BLA::Norm(gyro_int); + BLA::Matrix<3,1> axis = {(gyro_int(0) / rotVecNorm), (gyro_int(1) / rotVecNorm), (gyro_int(2) / rotVecNorm)}; + BLA::Matrix<4,1> dq = { - cos(rotVecNorm(0)/2.0f), - axis(0) * sinf(rotVecNorm(0)/2.0f), - axis(1) * sinf(rotVecNorm(0)/2.0f), - axis(2) * sinf(rotVecNorm(0)/2.0f), + cos(rotVecNorm/2.0f), + axis(0) * sinf(rotVecNorm/2.0f), + axis(1) * sinf(rotVecNorm/2.0f), + axis(2) * sinf(rotVecNorm/2.0f), }; BLA::Matrix<4,1> q = { @@ -262,10 +263,11 @@ BLA::Matrix<20,1> StateEstimator::fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix BLA::Matrix<3,1> v_dot = QuaternionUtils::quatToRot(q) * accel + g_i; BLA::Matrix<3,1> old_v = { x(QMEKFInds::v_x), x(QMEKFInds::v_y), x(QMEKFInds::v_z) - } + }; BLA::Matrix<3,1> old_p = { x(QMEKFInds::p_x), x(QMEKFInds::p_y), x(QMEKFInds::p_z) - } + }; + v = old_v + v_dot * pv_dt; p = old_p + v * pv_dt; @@ -275,9 +277,12 @@ BLA::Matrix<20,1> StateEstimator::fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix x(QMEKFInds::p_x) = p(0); x(QMEKFInds::p_y) = p(1); x(QMEKFInds::p_z) = p(2); + + return x; + } -BLA::Matrix<19, 1> StateEstimator::predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt) { +BLA::Matrix<19, 19> StateEstimator::predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt) { BLA::Matrix<3,3> gyroSkew = QuaternionUtils::skewSymmetric(gyroVec); BLA::Matrix<3,3> accelSkew = QuaternionUtils::skewSymmetric(accelVec); @@ -292,76 +297,84 @@ BLA::Matrix<19, 1> StateEstimator::predictionFunction(BLA::Matrix<19, 19> P_, BL BLA::Matrix<3, 3> rotMatrix = QuaternionUtils::quatToRot(q); BLA::Matrix<19, 19> F; - F = F.Fill(0); + F.Fill(0); //Row 1 - 3 - F.subMatrix<3, 3>(0, QMEKFInds::q_w) = -1 * gyroSkew; - F.subMatrix<3, 3>(0, QMEKFInds::gb_x) = -1 * I_3; + F.Submatrix<3, 3>(0, QMEKFInds::q_w) = -1.0f * gyroSkew; + F.Submatrix<3, 3>(0, QMEKFInds::gb_x) = -1.0f * I_3; //Row 4 - 6 - F.subMatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::q_w) = -1 * rotMatrix * accelSkew; - F.subMatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::ab_x) = -1 * rotMatrix; + F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::q_w) = -1.0f * rotMatrix * accelSkew; + F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::ab_x) = -1.0f * rotMatrix; //Row 7 - 9 - F.subMatrix<3, 3>(QMEKDInds::P_x -1, 3) = I_3; + F.Submatrix<3, 3>(QMEKFInds::p_x -1, 3) = I_3; BLA::Matrix<19, 19> phi; - phi = phi.Fill(0); + phi.Fill(0); - phi = I_19 + (F * dt) + (0.5 * F * F * pow(dt, 2)); + phi = I_19 + (F * dt) + (0.5f * F * F * float(pow(dt, 2))); BLA::Matrix<19, 19> phi_t = ~phi; BLA::Matrix<19, 19> Q_d; - Q_d = Q_d.Fill(0); + Q_d.Fill(0); BLA::Matrix<3, 3> gyro_var_diag; - gyro_var_diag = gyro_var_diag.Fill(0); + gyro_var_diag.Fill(0); gyro_var_diag(0, 0) = QMEKFInds::gyro_var; gyro_var_diag(1, 1) = QMEKFInds::gyro_var; - gryo_var_diag(2, 2) = QMEKFInds::gyro_var; + gyro_var_diag(2, 2) = QMEKFInds::gyro_var; BLA::Matrix<3, 3> gyro_bias_var_diag; - gyro_bias_var_diag = gyro_bias_var_diag.Fill(0); + gyro_bias_var_diag.Fill(0); gyro_bias_var_diag(0, 0) = QMEKFInds::gyro_bias_var; gyro_bias_var_diag(1, 1) = QMEKFInds::gyro_bias_var; gyro_bias_var_diag(2, 2) = QMEKFInds::gyro_bias_var; BLA::Matrix<3, 3> accel_bias_var_diag; - accel_bias_var_diag = accel_bias_var_diag.Fill(0); + accel_bias_var_diag.Fill(0); accel_bias_var_diag(0, 0) = QMEKFInds::accel_bias_var; accel_bias_var_diag(1, 1) = QMEKFInds::accel_bias_var; accel_bias_var_diag(2, 2) = QMEKFInds::accel_bias_var; - Q_d.subMatrix<3, 3>(QMEKFInds::q_w, QMEKFInds::q_w) = (gyro_var_diag * dt) + (gyro_bias_var_diag * (pow(dt, 3) / 10)); - Q_d.subMatrix<3, 3>(QMEKFInds::q_w, 9) = -1 * gyro_bias_var_diag * (pow(dt, 2) / 2); + Q_d.Submatrix<3, 3>(QMEKFInds::q_w, QMEKFInds::q_w) = (gyro_var_diag * dt) + (gyro_bias_var_diag * float((pow(dt, 3) / 10))); + Q_d.Submatrix<3, 3>(QMEKFInds::q_w, 9) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2)); - Q_d.subMatrix<3 ,3>(3, 3) = QMEKF::Inds::R_Grav * dt + aaccel_bias_var_diag * (pow(dt, 3) / 3); - Q_d.subMatrix<3, 3>(3, 6) = accel_bias_var_diag * (pow(dt ,4) / 8.0) + QMEKFInds::R_grav * (pow(dt, 2) / 2.0); - Q_d.subMatrix<3, 3>(3, 10) = -1.0 * accel_bias_var_diag * (pow(dt, 2) / 2.0); + BLA::Matrix<3, 3> R_grav_diag; + R_grav_diag.Fill(0); + R_grav_diag(0, 0) = QMEKFInds::R_grav; + R_grav_diag(1, 1) = QMEKFInds::R_grav; + R_grav_diag(2, 2) = QMEKFInds::R_grav; - Q_d.subMatrix<3, 3>(6, 3) = QMEKFInds::R_grav * (pow(dt, 2) / 2) + accel_bias_var_diag * (pow(dt, 4) / 8.0); - Q_d.subMatrix<3, 3>(6, 6) = QMEKFInds::R_grav * (pow(dt, 3) / 3.0) + accel_bias_var_diag * (pow(dt, 5) / 20.0); - Q_d.subMatrix<3, 3>(6, 10) = -1.0 * accel_bias_var_diag * (pow(dt, 3) / 6.0); + Q_d.Submatrix<3 ,3>(3, 3) = R_grav_diag * dt + accel_bias_var_diag * float((pow(dt, 3) / 3)); + Q_d.Submatrix<3, 3>(3, 6) = accel_bias_var_diag * float((pow(dt ,4) / 8.0)) + R_grav_diag * float((pow(dt, 2) / 2.0)); + Q_d.Submatrix<3, 3>(3, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); - Q_d.subMatix<3, 3>(9, 0) = -1.0 * gyro_bias_var_diag * (pow(dt, 2) / 2.0); - Q_d.subMatrix<3, 3>(9, 9) = gyro_bias_var_diag * (pow(dt, 2) / 2.0); + Q_d.Submatrix<3, 3>(6, 3) = R_grav_diag * float((pow(dt, 2) / 2)) + accel_bias_var_diag * float((pow(dt, 4) / 8.0)); + Q_d.Submatrix<3, 3>(6, 6) = R_grav_diag * float((pow(dt, 3) / 3.0)) + accel_bias_var_diag * float((pow(dt, 5) / 20.0)); + Q_d.Submatrix<3, 3>(6, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 3) / 6.0)); - Q_d.subMatrix<3, 3>(12, 3) = -1.0 * accel_bias_var_diag * (pow(dt, 2) / 2.0); - Q_d.subMatrix<3, 3>(12, 6) = -1.0 * accel_bias_var_diag * (pow(dt, 2) / 2.0); - Q_d.subMatrix<3, 3>(12, 12) = accel_bias_var_diag * dt; + Q_d.Submatrix<3, 3>(9, 0) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); + Q_d.Submatrix<3, 3>(9, 9) = gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); - Q_d(15, 15) = mag_bias_var * dt; + Q_d.Submatrix<3, 3>(12, 3) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); + Q_d.Submatrix<3, 3>(12, 6) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); + Q_d.Submatrix<3, 3>(12, 12) = accel_bias_var_diag * dt; - Q_d(18, 18) = baro_bias_var * dt; + Q_d(15, 15) = QMEKFInds::mag_bias_var * dt; + + Q_d(18, 18) = QMEKFInds::baro_bias_var * dt; BLA::Matrix<19, 19> P; - P = phi * P_ * phi_t + Q_D; + P = phi * P_ * phi_t + Q_d; + + return P; } -BLA::Matrix<20, 1> StateEstimator::runAccelUpdate(BLA::Matrix<3,1> accel_meas) +BLA::Matrix<20, 1> StateEstimator::runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3,1> accel_meas) { BLA::Matrix<4,1> q = { @@ -371,20 +384,20 @@ BLA::Matrix<20, 1> StateEstimator::runAccelUpdate(BLA::Matrix<3,1> accel_meas) x(QMEKFInds::q_z), }; - BLA::Matrix<3, 20> H_accel; - H_accel = H_accel.Fill(0); - H_accel.subMatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * (-1.0 * QMEKFInds::normal_i;)); - H_accel.subMatrix<3, 3>(0, QMEKFInds::ab_x - 1) = -1.0 * QuaternionUtils::quat2DCM(q); + BLA::Matrix<3, 19> H_accel; + H_accel.Fill(0); + H_accel.Submatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * (-1.0f * normal_i)); + H_accel.Submatrix<3, 3>(0, QMEKFInds::ab_x - 1) = -1.0f * QuaternionUtils::quat2DCM(q); - h_accel = QuaternionUtils::quat2DCM(q) * QMEKFInds::normal_i; - - BLA::Matrix<3, 3> R = subMatrix(R_all) + BLA::Matrix<3, 1> h_accel; - EKFCalcErrorInject(x, P, H_accel, h_accel, R); + h_accel = QuaternionUtils::quat2DCM(q) * normal_i; + + EKFCalcErrorInject(x, P, accel_meas, H_accel, h_accel, R_accel); } -BLA::Matrix<20, 1> StateEstimator::runMagUpdate(BLA::Matrix<3, 1> mag_meas) { +BLA::Matrix<20, 1> StateEstimator::runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> mag_meas) { // TODO input igrm model somehow figure out BLA::Matrix<4, 1> q = { @@ -394,57 +407,69 @@ BLA::Matrix<20, 1> StateEstimator::runMagUpdate(BLA::Matrix<3, 1> mag_meas) { x(QMEKFInds::q_z) }; - BLA::Matrix<3, 20> H_mag; - H_mag = H_mag.Fill(0); - H_mag.subMatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * igrm_model); - H_mag.subMatrix<3, 3>(0, QMEKFInds::gb_x) = I_3; + BLA::Matrix<3, 19> H_mag; + H_mag.Fill(0); + H_mag.Submatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * m_i); + H_mag.Submatrix<3, 3>(0, QMEKFInds::gb_x) = I_3; - h_mag = QuaternionUtils::quat2DCM(q) * igrm_model; + BLA::Matrix<3, 1> h_mag; - BLA::Matrix<3, 3> R = subMatrix(R_all); // IDK something + h_mag = QuaternionUtils::quat2DCM(q) * m_i; - EKFCalcErrorInject(x, P, mag_meas, H_mag, h_mag, R); + EKFCalcErrorInject(x, P, mag_meas, H_mag, h_mag, R_mag); } -BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<3, 1> gps_meas_ecef) { +BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps_meas_ecef) { BLA::Matrix<3, 1> pos_ned = QuaternionUtils::ecef2ned(gps_meas_ecef, launch_ecef, R_ET); - BLA::Matrix<3, 20> H_gps; - H_gps = H_gps.Fill(0); - H_gps.SubMatrix<3, 3>(0, QMEKFInds::gps_x) = I_3; + BLA::Matrix<3, 19> H_gps; + H_gps.Fill(0); + H_gps.Submatrix<3, 3>(0, QMEKFInds::p_x) = I_3; BLA::Matrix<3, 1> h_gps = { - x(QMEKFInds::gps_x), - x(QMEKFInds::gps_y), - x(QMEKFInds::gps_z), + x(QMEKFInds::p_x), + x(QMEKFInds::p_y), + x(QMEKFInds::p_z), }; - BLA::Matrix<3, 3> R = subMatrix(R_all); // not right, fix - - EKFCalcErrorInject(x, P, pos_ned, H_gps, h_gps, R); + EKFCalcErrorInject(x, P, pos_ned, H_gps, h_gps, R_gps); } -BLA::Matrix<20, 1> StateEstimator::EKFCalcErrorInject(BLA::Matrix<20, 1> oldState, BLA::Matrix<19, 19> oldP, BLA::Matrix sens_reading, BLA::Matrix H_matrix, BLA::Matrix h, BLA::Matrix R) { - residual = sens - h_matrix; +BLA::Matrix<20, 1> StateEstimator::EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R) { + BLA::Matrix<3, 1> residual; + residual = sens_reading - h; - S = H * oldP * ~H + R; - K = (oldP * ~H) * BLA::Inverse(S); + BLA::Matrix<3, 3> S; + BLA::Matrix<19, 3> K; + BLA::Matrix<19, 3> H_t = ~H; + + S = H * oldP * H_t + R; + K = (oldP * H_t) * BLA::Inverse(S); BLA::Matrix<19, 1> postErrorState = K * residual; // Inject error angles into quat - BLA::Matrix<3, 1> rotVec = 1.0 * posterioriErrorState(1:3); + BLA::Matrix<3, 1> alpha; + alpha = {postErrorState(0), postErrorState(1), postErrorState(2)}; + BLA::Matrix<3, 1> rotVec = 1.0f * alpha; float rotVecNorm = BLA::Norm(rotVec); BLA::Matrix<3,1> axis = rotVec / rotVecNorm; - BLA::Matrix<3,1> dq = + BLA::Matrix<4,1> dq = + { + cos(rotVecNorm/2.0f), + axis(0) * sinf(rotVecNorm/2.0f), + axis(1) * sinf(rotVecNorm/2.0f), + axis(2) * sinf(rotVecNorm/2.0f), + }; + BLA::Matrix<4, 1> old_q = { - cos(rotVecNorm(0)/2.0f) - axis(0) * sinf(rotVecNorm(0)/2.0f), - axis(1) * sinf(rotVecNorm(0)/2.0f), - axis(2) * sinf(rotVecNorm(0)/2.0f), + x(0), + x(1), + x(2), + x(3) }; - BLA::Matrix q = QuaternionUtils::quatMultiply(x(1:4), dq); + BLA::Matrix<4, 1> q = QuaternionUtils::quatMultiply(old_q, dq); // Set quats x(0) = q(0); @@ -470,7 +495,7 @@ BLA::Matrix<20, 1> StateEstimator::EKFCalcErrorInject(BLA::Matrix<20, 1> oldStat // Set accel bias x(13) = x(13) + postErrorState(12); x(14) = x(14) + postErrorState(13); - x(15) = x(15) + postErrorState()4; + x(15) = x(15) + postErrorState(14); // Set mag bias x(16) = x(16) + postErrorState(15); @@ -479,5 +504,7 @@ BLA::Matrix<20, 1> StateEstimator::EKFCalcErrorInject(BLA::Matrix<20, 1> oldStat // Set baro bias x(19) = x(19) + postErrorState(18); + + return x; } diff --git a/QMEKF/qmekf.h b/QMEKF/qmekf.h index aad5a5d..79a5be0 100644 --- a/QMEKF/qmekf.h +++ b/QMEKF/qmekf.h @@ -1,12 +1,7 @@ #pragma once #include "BasicLinearAlgebra.h" -#include -#include "boilerplate/Logging/Loggable.h" -#include "boilerplate/Sensors/Impl/ICM20948.h" -#include "boilerplate/Sensors/Impl/LPS22.h" -#include "boilerplate/Sensors/Impl/MAX10S.h" -#include "boilerplate/TimedPointer/TimedPointer.h" + #include "kfConsts.h" #include @@ -58,6 +53,7 @@ constexpr float accel_bias_var = pow(0.00098 * 9.8, 2); constexpr float mag_bias_var = 25; constexpr float baro_bias_var = pow(7.5, 2); +constexpr float R_grav = pow(sqrt(asm330_const::accelXY_var) * 9.8, 2); }; // namespace QMEKFInds @@ -142,18 +138,18 @@ class StateEstimator { // BLA::Matrix<3, 1> accel); BLA::Matrix<20,1> fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt); - BLA::Matrix<19, 1> predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt); + BLA::Matrix<19, 19> predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt); // Update Functions - void runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> a_b); + BLA::Matrix<20,1> runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> a_b); - void runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> m_b); + BLA::Matrix<20,1> runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> m_b); - void runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps); + BLA::Matrix<20,1> runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps); // void runBaroUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<1, 1> baro); - void EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 20> H_matrix, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R); + BLA::Matrix<20,1> EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R); // State Vector Allocation BLA::Matrix<20, 1> x_min; @@ -173,6 +169,22 @@ class StateEstimator { BLA::Matrix<19, 19> I_19 = BLA::Eye<19, 19>(); BLA::Matrix<3, 3> I_3 = BLA::Eye<3, 3>(); + //R values + float accel_var = pow(sqrt(asm330_const::accelXY_var) * 9.8, 2); + float mag_var = icm20948_const::magXYZ_var; + float gps_var = Max10S_const::gpsXYZ_var; + //R matricies + BLA::Matrix<3, 3> R_accel = {accel_var, 0, 0, + 0, accel_var, 0, + 0, 0, accel_var}; + + BLA::Matrix<3, 3> R_mag = {mag_var, 0, 0, + 0, mag_var, 0, + 0, 0, mag_var}; + + BLA::Matrix<3, 3> R_gps= {gps_var, 0, 0, + 0, gps_var, 0, + 0, 0, gps_var}; BLA::Matrix<10, 1> R_all = { pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), From c35401a4cdd07ce9068f3a4c7248a1d7374d8a25 Mon Sep 17 00:00:00 2001 From: krizzyegg Date: Wed, 19 Nov 2025 20:29:25 -0500 Subject: [PATCH 03/14] Added utils --- Utilities/QuaternionUtils.cpp | 165 ++++++++++++++++++++++++++++++++++ Utilities/QuaternionUtils.h | 43 +++++++++ 2 files changed, 208 insertions(+) create mode 100644 Utilities/QuaternionUtils.cpp create mode 100644 Utilities/QuaternionUtils.h diff --git a/Utilities/QuaternionUtils.cpp b/Utilities/QuaternionUtils.cpp new file mode 100644 index 0000000..8c0bb9d --- /dev/null +++ b/Utilities/QuaternionUtils.cpp @@ -0,0 +1,165 @@ +#include "QuaternionUtils.h" +#include "BasicLinearAlgebra.h" +#include + +BLA::Matrix<3, 3> QuaternionUtils::quatToRot(const BLA::Matrix<4, 1> &quat) { + float w = quat(0); + float x = quat(1); + float y = quat(2); + float z = quat(3); + + BLA::Matrix<3, 3> rot; + + // Row1 + rot(0, 0) = 1 - 2 * y * y - 2 * z * z; + rot(0, 1) = 2 * x * y - 2 * w * z; + rot(0, 2) = 2 * x * z + 2 * w * y; + + // Row2 + rot(1, 0) = 2 * x * y + 2 * w * z; + rot(1, 1) = 1 - 2 * x * x - 2 * z * z; + rot(1, 2) = 2 * y * z - 2 * w * x; + + // Row3 + rot(2, 0) = 2 * x * z - 2 * w * y; + rot(2, 1) = 2 * y * z + 2 * w * x; + rot(2, 2) = 1 - 2 * x * x - 2 * y * y; + + return rot; +} + +BLA::Matrix<3, 3> QuaternionUtils::quat2DCM(const BLA::Matrix<4, 1> &quat) { + float w = quat(0); + float x = quat(1); + float y = quat(2); + float z = quat(3); + + BLA::Matrix<3, 3> rot; + + // Row1 + rot(0, 0) = w * w + x * x - y * y - z * z; + rot(0, 1) = 2 * x * y + 2 * w * z; + rot(0, 2) = 2 * x * z - 2 * w * y; + + // Row2 + rot(1, 0) = 2 * x * y - 2 * w * z; + rot(1, 1) = w * w - x * x + y * y - z * z; + rot(1, 2) = 2 * y * z + 2 * w * x; + + // Row3 + rot(2, 0) = 2 * x * z + 2 * w * y; + rot(2, 1) = 2 * y * z - 2 * w * x; + rot(2, 2) = w * w - x * x - y * y + z * z; + + return rot; +} + +BLA::Matrix<3, 1> QuaternionUtils::getUpVector(const BLA::Matrix<3, 3> &rot) { + return BLA::Matrix<3, 1>(rot(0, 1), rot(1, 1), rot(2, 1)); +} + +BLA::Matrix<3, 1> QuaternionUtils::getForwardVector(const BLA::Matrix<3, 3> &rot) { + return BLA::Matrix<3, 1>(rot(0, 2), rot(1, 2), rot(2, 2)); +} + +BLA::Matrix<3,1> QuaternionUtils::getRightVector(const BLA::Matrix<3,3> &rot) { + return BLA::Matrix<3,1>(rot(0,0), rot(1,0), rot(2,0)); +} + +BLA::Matrix<3, 3> QuaternionUtils::skewSymmetric(const BLA::Matrix<3, 1> &vec) { + BLA::Matrix<3, 3> mat; + + mat(0, 0) = 0; + mat(0, 1) = -1 * vec(2, 0); + mat(0, 2) = vec(1, 0); + + mat(1, 0) = vec(2, 0); + mat(1, 1) = 0; + mat(1, 2) = -1 * vec(0, 0); + + mat(2, 0) = -1 * vec(1, 0); + mat(2, 1) = vec(0, 0); + mat(2, 2) = 0; + + return mat; + +} + + +BLA::Matrix<4, 1> QuaternionUtils::rotVec2Quat(const BLA::Matrix<3, 1> &vec) { + BLA::Matrix<4, 1> quat; + + float norm = BLA::Norm(vec); + BLA::Matrix<3, 1> vec_rot_normed = vec / norm; + + quat(0, 0) = cos(norm / 2); + quat(1, 0) = vec_rot_normed(0, 0) * sin(norm / 2); + quat(2, 0) = vec_rot_normed(1, 0) * sin(norm / 2); + quat(3, 0) = vec_rot_normed(2, 0) * sin(norm / 2); + + return quat; +} + +BLA::Matrix<4, 1> QuaternionUtils::quatMultiply(const BLA::Matrix<4, 1> &p, const BLA::Matrix<4, 1> &q) { + BLA::Matrix<4, 1> quat; + + quat(0, 0) = p(0, 0) * q(0, 0) - p(1, 0) * q(1, 0) - p(2, 0) * q(2, 0) - p(3, 0) * q(3, 0); + quat(1, 0) = p(0, 0) * q(1, 0) + p(1, 0) * q(0, 0) + p(2, 0) * q(3, 0) - p(3, 0) * q(2, 0); + quat(2, 0) = p(0, 0) * q(2, 0) - p(1, 0) * q(3, 0) + p(2, 0) * q(1, 0) + p(3, 0) * q(1, 0); + quat(3, 0) = p(0, 0) * q(3, 0) + p(1, 0) * q(2, 0) - p(2, 0) * q(1, 0) + p(3, 0) * q(3, 0); + + + return quat; +} + +BLA::Matrix<3, 1> QuaternionUtils::lla2ecef(const BLA::Matrix<3, 1> &lla) { + float pi = 3.141592; + float lat_rad = lla(0) * pi / 180.0; + float lon_rad = lla(1) * pi / 180.0; + + float a = 6378.0 * 1000.0; + float b = 6357.0 * 1000.0; + + float e = std::sqrt(1.0 - (std::pow(b, 2) / std::pow(a, 2))); + + float N = a / std::sqrt(1.0 - std::pow(e, 2) * std::pow(sin(lat_rad), 2)); + + float x = (N + lla(2)) * cos(lat_rad) * cos(lon_rad); + float y = (N + lla(2)) * cos(lat_rad) * sin(lon_rad); + float z = ((1 - std::pow(e, 2)) * N + lla(2)) * sin(lat_rad); + + BLA::Matrix<3, 1> ecef = {x, y, z}; + + return ecef; + + +} + +BLA::Matrix<3, 3> QuaternionUtils::dcm_ned2ecef(const BLA::Matrix<2, 1> &ll) { + float pi = 3.141592653; + float lat_rads = ll(0) * (pi / 180.0); + float lon_rads = ll(1) * (pi / 180.0); + BLA::Matrix<3, 3> R_ET = { + -1.0 * sin(lat_rads) * cos(lon_rads), -1.0 * sin(lon_rads), -1.0 * cos(lat_rads) * cos(lon_rads), + -1.0 * sin(lat_rads) * sin(lon_rads), cos(lon_rads), -1.0 * cos(lat_rads) * sin(lon_rads), + cos(lat_rads), 0, -1.0 * sin(lat_rads) + }; + return R_ET; +} + +BLA::Matrix<3, 1> QuaternionUtils::ecef2ned(const BLA::Matrix<3, 1> &ecef_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET) { + return ~R_ET * (ecef_meas - launch_ecef); + +} + + + +BLA::Matrix<4, 1> quatConjugate(const BLA::Matrix<4, 1> &p){ + BLA::Matrix<4, 1> quat; + quat(0, 0) = p(0); + quat(1, 0) = -1 * p(1); + quat(2, 0) = -1 * p(2); + quat(3, 0) = -1 * p(3); + + return quat; +} diff --git a/Utilities/QuaternionUtils.h b/Utilities/QuaternionUtils.h new file mode 100644 index 0000000..2619ced --- /dev/null +++ b/Utilities/QuaternionUtils.h @@ -0,0 +1,43 @@ +#pragma once + +#include "BasicLinearAlgebra.h" + +namespace QuaternionUtils { + // Convert quaternion (w,x,y,z) to rotation matrix + // quat SHOULD be normalized + BLA::Matrix<3,3> quatToRot(const BLA::Matrix<4,1> &quat); + + // Quaternion to DCM + BLA::Matrix<3, 3> quat2DCM(const BLA::Matrix<4, 1> &quat); + + // Get the up vector from the rotation matrix for the payload + // In NED coordinates, this will be the second column of the rotation matrix + BLA::Matrix<3,1> getUpVector(const BLA::Matrix<3,3> &rot); + + // Get the forward vector from the rotation matrix for the payload + // NED coord == third col + BLA::Matrix<3,1> getForwardVector(const BLA::Matrix<3,3> &rot); + + // Get the right vector from the rotation matrix for the payload + // NED coord == first col + BLA::Matrix<3,1> getRightVector(const BLA::Matrix<3,3> &rot); + + // Get skew symmetric of a 3x1 vector + BLA::Matrix<3, 3> skewSymmetric(const BLA::Matrix<3, 1> &vec); + + // Rotation vector to quaternion + BLA::Matrix<4, 1> rotVec2Quat(const BLA::Matrix<3, 1> &vec); + + // Quaternion multiply + BLA::Matrix<4, 1> quatMultiply(const BLA::Matrix<4, 1> &p, const BLA::Matrix<4, 1> &q); + + BLA::Matrix<3, 1> lla2ecef(const BLA::Matrix<3, 1> &lla); + + BLA::Matrix<3, 3> dcm_ned2ecef(const BLA::Matrix<2, 1> &ll); + + BLA::Matrix<3, 1> ecef2ned(const BLA::Matrix<3, 1> &ecef_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET); + + //Quaternion Conjugate + BLA::Matrix<4, 1> quatConjugate(const BLA::Matrix<4, 1> &p); + +} From e3ff379f557b64b0bababb3a3a8bd987ce42a18e Mon Sep 17 00:00:00 2001 From: Abhay Mathur <66543532+abhayma1000@users.noreply.github.com> Date: Wed, 3 Dec 2025 13:25:21 -0500 Subject: [PATCH 04/14] Made into platformio package, wrote test cases --- .gitignore | 5 + include/README | 37 ++ kfConsts.h | 28 -- lib/README | 46 +++ platformio.ini | 30 ++ src/Conventions.txt | 31 ++ {Utilities => src}/QuaternionUtils.cpp | 119 ++++-- {Utilities => src}/QuaternionUtils.h | 20 +- src/kfConsts.h | 57 +++ src/main.cpp | 88 +++++ src/qmekf.cpp | 510 +++++++++++++++++++++++++ src/qmekf.h | 190 +++++++++ test/README | 11 + 13 files changed, 1119 insertions(+), 53 deletions(-) create mode 100644 .gitignore create mode 100644 include/README delete mode 100644 kfConsts.h create mode 100644 lib/README create mode 100644 platformio.ini create mode 100644 src/Conventions.txt rename {Utilities => src}/QuaternionUtils.cpp (57%) rename {Utilities => src}/QuaternionUtils.h (60%) create mode 100644 src/kfConsts.h create mode 100644 src/main.cpp create mode 100644 src/qmekf.cpp create mode 100644 src/qmekf.h create mode 100644 test/README diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..89cc49c --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +.pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch diff --git a/include/README b/include/README new file mode 100644 index 0000000..49819c0 --- /dev/null +++ b/include/README @@ -0,0 +1,37 @@ + +This directory is intended for project header files. + +A header file is a file containing C declarations and macro definitions +to be shared between several project source files. You request the use of a +header file in your project source file (C, C++, etc) located in `src` folder +by including it, with the C preprocessing directive `#include'. + +```src/main.c + +#include "header.h" + +int main (void) +{ + ... +} +``` + +Including a header file produces the same results as copying the header file +into each source file that needs it. Such copying would be time-consuming +and error-prone. With a header file, the related declarations appear +in only one place. If they need to be changed, they can be changed in one +place, and programs that include the header file will automatically use the +new version when next recompiled. The header file eliminates the labor of +finding and changing all the copies as well as the risk that a failure to +find one copy will result in inconsistencies within a program. + +In C, the convention is to give header files names that end with `.h'. + +Read more about using header files in official GCC documentation: + +* Include Syntax +* Include Operation +* Once-Only Headers +* Computed Includes + +https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html diff --git a/kfConsts.h b/kfConsts.h deleted file mode 100644 index a3b19e3..0000000 --- a/kfConsts.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -constexpr static float g = 9.80665; // [m/s/s] Earth's Grav Accel - -constexpr static struct { - float accelXY_var = 0.0020f; // [g] - float accelZ_var = 0.0014f; // [g] - float accelXY_VRW = 0.0003f; // [g/sqrt(Hz)] - float accelZ_VRW = 0.0002f; // [g/sqrt(Hz)] - float gyroX_var = 0.0947f; // [deg/s] - float gyroY_var = 0.1474f; // [deg/s] - float gyroZ_var = 0.2144f; // [deg/s] - float gyro_VRW = 0.0241f; // [deg/s/sqrt(Hz)] -} asm330_const; - -constexpr static struct { - float baro_var = 0.0f; -} lps22_const; - -constexpr static struct { - float accelXY_var = 0.0383f; // [g] - float accelZ_var = 0.0626f; // [g] - float accelXY_VRW = 0.0062f; // [g/sqrt(Hz)] - float accelZ_VRW = 0.0099f; // [g/sqrt(Hz)] - float gyroXYZ_var = 0.0051f; // [deg/s] - float gyro_VRW = 8.33e-4f; // [deg/s/sqrt(Hz)] - float magXYZ_var = 0.7263f; // [uT] -} icm20948_const; \ No newline at end of file diff --git a/lib/README b/lib/README new file mode 100644 index 0000000..9379397 --- /dev/null +++ b/lib/README @@ -0,0 +1,46 @@ + +This directory is intended for project specific (private) libraries. +PlatformIO will compile them to static libraries and link into the executable file. + +The source code of each library should be placed in a separate directory +("lib/your_library_name/[Code]"). + +For example, see the structure of the following example libraries `Foo` and `Bar`: + +|--lib +| | +| |--Bar +| | |--docs +| | |--examples +| | |--src +| | |- Bar.c +| | |- Bar.h +| | |- library.json (optional. for custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html +| | +| |--Foo +| | |- Foo.c +| | |- Foo.h +| | +| |- README --> THIS FILE +| +|- platformio.ini +|--src + |- main.c + +Example contents of `src/main.c` using Foo and Bar: +``` +#include +#include + +int main (void) +{ + ... +} + +``` + +The PlatformIO Library Dependency Finder will find automatically dependent +libraries by scanning project source files. + +More information about PlatformIO Library Dependency Finder +- https://docs.platformio.org/page/librarymanager/ldf.html diff --git a/platformio.ini b/platformio.ini new file mode 100644 index 0000000..23d465b --- /dev/null +++ b/platformio.ini @@ -0,0 +1,30 @@ +; PlatformIO Project Configuration File +; +; Build options: build flags, source filter +; Upload options: custom upload port, speed and extra flags +; Library options: dependencies, extra library storages +; Advanced options: extra scripting +; +; Please visit documentation for the other options and examples +; https://docs.platformio.org/page/projectconf.html + +[env:disco_g031j6] +platform = ststm32 +board = disco_g031j6 +framework = arduino + +lib_deps = + adafruit/Adafruit LPS2X@^2.0.6 + adafruit/Adafruit BusIO@^1.17.0 + adafruit/Adafruit Unified Sensor@^1.1.15 + stm32duino/STM32duino ASM330LHH@^2.0.1 + adafruit/Adafruit ICM20X@^2.0.7 + sparkfun/SparkFun u-blox GNSS v3@^3.1.8 + greiman/SdFat + nanopb/Nanopb@^0.4.91 + tomstewart89/BasicLinearAlgebra @ ^5.1 + Wire + SPI + Servo + IWatchdog + diff --git a/src/Conventions.txt b/src/Conventions.txt new file mode 100644 index 0000000..44fc70f --- /dev/null +++ b/src/Conventions.txt @@ -0,0 +1,31 @@ +Notes: + + +I change magData variable to imuData because it contains all the IMU data, not just magData +Also please change the accel to return the m/s^2, not g's +I change take in int variable state to onLoop, not the boolean: + 0 = preLaunch + 1 = high thrust + 2 = coast + 3 = falling down + + + +Frequencies variable in format: + priori step + accel upate + mag update + gps update + baro update + + + + + +BLA library: + +* Norm: BLA::Norm(vec); +* BLA::CrossProduct(A, B); +* Define: BLA::Matrix<2, 3> x = {1, 2, 3, + 4, 5, 6,} +* Multiplication, addition is just * and + diff --git a/Utilities/QuaternionUtils.cpp b/src/QuaternionUtils.cpp similarity index 57% rename from Utilities/QuaternionUtils.cpp rename to src/QuaternionUtils.cpp index 8c0bb9d..e948398 100644 --- a/Utilities/QuaternionUtils.cpp +++ b/src/QuaternionUtils.cpp @@ -28,6 +28,20 @@ BLA::Matrix<3, 3> QuaternionUtils::quatToRot(const BLA::Matrix<4, 1> &quat) { return rot; } +BLA::Matrix<3, 1> QuaternionUtils::getUpVector(const BLA::Matrix<3, 3> &rot) { + return BLA::Matrix<3, 1>(rot(0, 1), rot(1, 1), rot(2, 1)); +} + +BLA::Matrix<3, 1> QuaternionUtils::getForwardVector(const BLA::Matrix<3, 3> &rot) { + return BLA::Matrix<3, 1>(rot(0, 2), rot(1, 2), rot(2, 2)); +} + +BLA::Matrix<3,1> QuaternionUtils::getRightVector(const BLA::Matrix<3,3> &rot) { + return BLA::Matrix<3,1>(rot(0,0), rot(1,0), rot(2,0)); +} + + + BLA::Matrix<3, 3> QuaternionUtils::quat2DCM(const BLA::Matrix<4, 1> &quat) { float w = quat(0); float x = quat(1); @@ -54,18 +68,6 @@ BLA::Matrix<3, 3> QuaternionUtils::quat2DCM(const BLA::Matrix<4, 1> &quat) { return rot; } -BLA::Matrix<3, 1> QuaternionUtils::getUpVector(const BLA::Matrix<3, 3> &rot) { - return BLA::Matrix<3, 1>(rot(0, 1), rot(1, 1), rot(2, 1)); -} - -BLA::Matrix<3, 1> QuaternionUtils::getForwardVector(const BLA::Matrix<3, 3> &rot) { - return BLA::Matrix<3, 1>(rot(0, 2), rot(1, 2), rot(2, 2)); -} - -BLA::Matrix<3,1> QuaternionUtils::getRightVector(const BLA::Matrix<3,3> &rot) { - return BLA::Matrix<3,1>(rot(0,0), rot(1,0), rot(2,0)); -} - BLA::Matrix<3, 3> QuaternionUtils::skewSymmetric(const BLA::Matrix<3, 1> &vec) { BLA::Matrix<3, 3> mat; @@ -85,7 +87,6 @@ BLA::Matrix<3, 3> QuaternionUtils::skewSymmetric(const BLA::Matrix<3, 1> &vec) { } - BLA::Matrix<4, 1> QuaternionUtils::rotVec2Quat(const BLA::Matrix<3, 1> &vec) { BLA::Matrix<4, 1> quat; @@ -100,6 +101,22 @@ BLA::Matrix<4, 1> QuaternionUtils::rotVec2Quat(const BLA::Matrix<3, 1> &vec) { return quat; } +BLA::Matrix<4, 1> QuaternionUtils::smallAngleRotVec2Quat(const BLA::Matrix<3, 1> &vec) { + BLA::Matrix<4, 1> quat; + quat(0, 0) = 1.0; + quat(1, 0) = 0.5 * vec(0, 0); + quat(2, 0) = 0.5 * vec(1, 0); + quat(3, 0) = 0.5 * vec(2, 0); + + return quat; +} + +BLA::Matrix<4, 1> QuaternionUtils::dcm2quat(const BLA::Matrix<3, 3> &dcm) { + // TODO + BLA::Matrix<4, 1> tmp_q = {1, 0, 0, 0}; + return tmp_q; +} + BLA::Matrix<4, 1> QuaternionUtils::quatMultiply(const BLA::Matrix<4, 1> &p, const BLA::Matrix<4, 1> &q) { BLA::Matrix<4, 1> quat; @@ -135,26 +152,24 @@ BLA::Matrix<3, 1> QuaternionUtils::lla2ecef(const BLA::Matrix<3, 1> &lla) { } -BLA::Matrix<3, 3> QuaternionUtils::dcm_ned2ecef(const BLA::Matrix<2, 1> &ll) { - float pi = 3.141592653; - float lat_rads = ll(0) * (pi / 180.0); - float lon_rads = ll(1) * (pi / 180.0); +BLA::Matrix<3, 3> QuaternionUtils::dcm_ned2ecef(const BLA::Matrix<3, 1> &lla) { + float pi = 3.141592653; // TODO declare this somewhere else but idk c++ + float lat_rads = lla(0) * (pi / 180.0); + float lon_rads = lla(1) * (pi / 180.0); BLA::Matrix<3, 3> R_ET = { - -1.0 * sin(lat_rads) * cos(lon_rads), -1.0 * sin(lon_rads), -1.0 * cos(lat_rads) * cos(lon_rads), - -1.0 * sin(lat_rads) * sin(lon_rads), cos(lon_rads), -1.0 * cos(lat_rads) * sin(lon_rads), + -1.0f * sin(lat_rads) * cos(lon_rads), -1.0f * sin(lon_rads), -1.0f * cos(lat_rads) * cos(lon_rads), + -1.0f * sin(lat_rads) * sin(lon_rads), cos(lon_rads), -1.0f * cos(lat_rads) * sin(lon_rads), cos(lat_rads), 0, -1.0 * sin(lat_rads) }; return R_ET; } -BLA::Matrix<3, 1> QuaternionUtils::ecef2ned(const BLA::Matrix<3, 1> &ecef_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET) { +BLA::Matrix<3, 1> QuaternionUtils::ecef2nedVec(const BLA::Matrix<3, 1> &ecef_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET) { return ~R_ET * (ecef_meas - launch_ecef); } - - -BLA::Matrix<4, 1> quatConjugate(const BLA::Matrix<4, 1> &p){ +BLA::Matrix<4, 1> QuaternionUtils::quatConjugate(const BLA::Matrix<4, 1> &p){ BLA::Matrix<4, 1> quat; quat(0, 0) = p(0); quat(1, 0) = -1 * p(1); @@ -163,3 +178,61 @@ BLA::Matrix<4, 1> quatConjugate(const BLA::Matrix<4, 1> &p){ return quat; } + +BLA::Matrix<3, 1> QuaternionUtils::g_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef) { + // TODO one day put in the world model + BLA::Matrix<3, 1> grav_ned = {0, 0, 9.8}; + return ~dcm_ned2ecef * grav_ned; +} + +BLA::Matrix<3, 1> QuaternionUtils::m_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef) { + // TODO one day put in the world model. School vals for now + BLA::Matrix<3, 1> mag_ned = {19.983111, -4.8716, 46.9986145}; + return ~dcm_ned2ecef * mag_ned; +} + +BLA::Matrix<3, 1> QuaternionUtils::normal_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef) { + // TODO one day put in the world model + BLA::Matrix<3, 1> normal_ned = {0, 0, -9.8}; + return ~dcm_ned2ecef * normal_ned; +} + +BLA::Matrix<3, 3> QuaternionUtils::vecs2mat(const BLA::Matrix<3, 1> v1, const BLA::Matrix<3, 1> v2, const BLA::Matrix<3, 1> v3) { + BLA::Matrix<3, 3> ret = {v1(0, 0), v2(0), v3(0, 0), + v1(1, 0), v2(1, 0), v3(1, 0), + v1(2, 0), v2(2, 0), v3(2, 0)}; + + return ret; +} + +// 1 is important, 2 is secondary +BLA::Matrix<4, 1> QuaternionUtils::triad_BE(const BLA::Matrix<3, 1> v1_b, const BLA::Matrix<3, 1> v2_b, const BLA::Matrix<3, 1> v1_i, const BLA::Matrix<3, 1> v2_i) { + BLA::Matrix<3, 1> v1_b_norm = v1_b / BLA::Norm(v1_b); + BLA::Matrix<3, 1> v2_b_norm = v1_b / BLA::Norm(v2_b); + BLA::Matrix<3, 1> v1_i_norm = v1_b / BLA::Norm(v1_i); + BLA::Matrix<3, 1> v2_i_norm = v1_b / BLA::Norm(v2_i); + + // Inertial + BLA::Matrix<3, 1> q_r = v1_i; + BLA::Matrix<3, 1> r_r = BLA::CrossProduct(v1_i, v2_i) / BLA::Norm(BLA::CrossProduct(v1_i, v2_i)); + BLA::Matrix<3, 1> s_r = BLA::CrossProduct(q_r, r_r); + + BLA::Matrix<3, 3> M_r = vecs2mat(q_r, r_r, s_r); + + + // Body + BLA::Matrix<3, 1> q_b = v1_b; + BLA::Matrix<3, 1> r_b = BLA::CrossProduct(v1_b, v2_b) / BLA::Norm(BLA::CrossProduct(v1_b, v2_b)); + BLA::Matrix<3, 1> s_b = BLA::CrossProduct(q_b, r_b); + + BLA::Matrix<3, 3> M_b = vecs2mat(q_b, r_b, s_b); + + BLA::Matrix<3, 3> R_BE = ~(M_r * ~M_b); + + return dcm2quat(R_BE); + + + + +} + diff --git a/Utilities/QuaternionUtils.h b/src/QuaternionUtils.h similarity index 60% rename from Utilities/QuaternionUtils.h rename to src/QuaternionUtils.h index 2619ced..564cd2a 100644 --- a/Utilities/QuaternionUtils.h +++ b/src/QuaternionUtils.h @@ -28,16 +28,32 @@ namespace QuaternionUtils { // Rotation vector to quaternion BLA::Matrix<4, 1> rotVec2Quat(const BLA::Matrix<3, 1> &vec); + // Small angle rot vec to quat + BLA::Matrix<4, 1> smallAngleRotVec2Quat(const BLA::Matrix<3, 1> &vec); + + BLA::Matrix<4, 1> dcm2quat(const BLA::Matrix<3, 3> &dcm); + // Quaternion multiply BLA::Matrix<4, 1> quatMultiply(const BLA::Matrix<4, 1> &p, const BLA::Matrix<4, 1> &q); BLA::Matrix<3, 1> lla2ecef(const BLA::Matrix<3, 1> &lla); - BLA::Matrix<3, 3> dcm_ned2ecef(const BLA::Matrix<2, 1> &ll); + BLA::Matrix<3, 3> dcm_ned2ecef(const BLA::Matrix<3, 1> &lla); - BLA::Matrix<3, 1> ecef2ned(const BLA::Matrix<3, 1> &ecef_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET); + BLA::Matrix<3, 1> ecef2nedVec(const BLA::Matrix<3, 1> &ecef_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET); //Quaternion Conjugate BLA::Matrix<4, 1> quatConjugate(const BLA::Matrix<4, 1> &p); + BLA::Matrix<3, 1> g_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef); + + BLA::Matrix<3, 1> m_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef); + + BLA::Matrix<3, 1> normal_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef); + + BLA::Matrix<3, 3> vecs2mat(const BLA::Matrix<3, 1> v1, const BLA::Matrix<3, 1> v2, const BLA::Matrix<3, 1> v3); + + BLA::Matrix<4, 1> triad_BE(const BLA::Matrix<3, 1> v1_b, const BLA::Matrix<3, 1> v2_b, const BLA::Matrix<3, 1> v1_i, const BLA::Matrix<3, 1> v2_i); + + } diff --git a/src/kfConsts.h b/src/kfConsts.h new file mode 100644 index 0000000..4bb84b3 --- /dev/null +++ b/src/kfConsts.h @@ -0,0 +1,57 @@ +#pragma once + +#include "BasicLinearAlgebra.h" +#include +constexpr static float g = 9.80665; // [m/s/s] Earth's Grav Accel + +constexpr float square(float a) { return a * a; } + +namespace asm330_const { +constexpr float accelXY_var = square(0.0020f); // [g] +constexpr float accelZ_var = square(0.0014f); // [g] +constexpr float accelXY_VRW = 0.0003f; // [g/sqrt(Hz)] +constexpr float accelZ_VRW = 0.0002f; // [g/sqrt(Hz)] +constexpr float gyroX_var = square(0.0947f); // [deg/s] +constexpr float gyroY_var = square(0.1474f); // [deg/s] +constexpr float gyroZ_var = square(0.2144f); // [deg/s] +constexpr float gyro_VRW = 0.0241f; // [deg/s/sqrt(Hz)] +}; // namespace asm330_const + +namespace lps22_const { +constexpr float baro_var = 0.0f; +}; + +namespace icm20948_const { +constexpr float accelXY_var = square(0.0383f); // [g] +constexpr float accelZ_var = square(0.0626f); // [g] +constexpr float accelXY_VRW = 0.0062f; // [g/sqrt(Hz)] +constexpr float accelZ_VRW = 0.0099f; // [g/sqrt(Hz)] +// constexpr float gyroXYZ_var = square(0.0051); // [rad/s] +constexpr float gyroXYZ_var = square(5e-4); // [rad/s] +constexpr float gyro_VRW = 8.33e-4f; // [rad/s/sqrt(Hz)] +constexpr float magXYZ_var = square(0.7263f); // [uT] +constexpr float quatVar = 0.3; // Idk Guess + + +}; // namespace icm20948_const + + +namespace Max10S_const { + constexpr float gpsXYZ_var = square(2.0); // [m] idk guess + constexpr float baro_var = square(7.5); // [P] idk guess +} + +namespace vimu_const { + // TODO vimu/2imu + + constexpr float gyro_var = 0.0051; + constexpr float gyro_bias_var = square(4.9e-5f * 9.8f); + + BLA::Matrix<3, 1> accel_var = {square(4.9e-5f * 9.9f), square(4.9e-5f * 9.9f), square(4.9e-5f * 9.9f)}; + constexpr float accel_bias_var = square(0.00098f * 9.8f); + + constexpr float mag_bias_var = 25; + constexpr float baro_bias_var = square(7.5f); + + +} diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..450c236 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,88 @@ +#include + +#include "BasicLinearAlgebra.h" + +#include "QuaternionUtils.h" + +void setup() { + // Since basing this whole c++ impl on the simulink, using that as reference here + Serial.println("Hello"); + + BLA::Matrix<4, 1> random_quat = {0.9344, 0.2435, 0.2435, 0.0907}; + BLA::Matrix<4, 1> random_quat2 = {0.3666, 0.4564, 0.7857, 0.1998}; + BLA::Matrix<3, 3> random_mat = {0.8662, 0.0206,0.2123, 0.6011, 0.9699, 0.1818, 0.7081, 0.8324, 0.1834}; + BLA::Matrix<3, 1> random_vec = {0.3042, 0.5248, 0.4319}; + BLA::Matrix<3, 1> school_lla = {42.274027, -71.811788, 10}; + + Serial.println("Test quat2dcm: Quat: {0.9344, 0.2435, 0.2435, 0.0907}. Expected DCM: \ + 0.8650 0.2880 -0.4109 \ + -0.0508 0.8649 0.4993 \ + 0.4992 -0.4110 0.7628. Actual DCM:"); + + Serial.println(QuaternionUtils::quat2DCM(random_quat)); + + + + Serial.println("Test skewSymmetric: Random vec: {0.3042, 0.5248, 0.4319}. Expected Mat: \ + 0 -0.4319 0.5248 \ + 0.4319 0 -0.3042 \ + -0.5248 0.3042 0"); + + Serial.println(QuaternionUtils::skewSymmetric(random_vec)); + + + + Serial.println("Test rotVec2Quat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {0.9315, 0.2769, 0.4776, 0.3931}"); + + Serial.println(QuaternionUtils::rotVec2Quat(random_vec)); + + + + Serial.println("Test smallAngleRotVec2Quat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {1.0000, 0.1521, 0.2624, 0.2160}"); + + Serial.println(QuaternionUtils::smallAngleRotVec2Quat(random_vec)); + + + +// TODO eventually test dcm2quat + + + Serial.println("Test quatMultiply: Random Quats: {0.9344 0.2435 0.2435 0.0907} and {0.3666 0.4564 0.7857 0.1998} \ + Expected Quat: {0.0220 0.4932 0.8162 0.3002}"); + + Serial.println(QuaternionUtils::quatMultiply(random_quat, random_quat2)); + + + Serial.println("Test lla2ecef: School LLA: {42.274027, -71.811788, 10}. Expected ECEF: {1475354, -4490428, 4268181}"); + + Serial.println(QuaternionUtils::lla2ecef(school_lla)); + + + Serial.println("Test dcm_ned2ecef: School LLA: {42.274027, -71.811788, 10}. Expected DCM: \ + -0.2100 0.9500 -0.2310 \ + 0.6391 0.3121 0.7030 \ + 0.7399 0 -0.6727"); + Serial.println(QuaternionUtils::dcm_ned2ecef(school_lla)); + + + Serial.println("Test ecef2nedVec:"); // Eh one day + + Serial.println("Test quatConj: Test quat: {0.9344 0.2435 0.2435 0.0907}. Expected quat: 0.9344 -0.2435 -0.2435 -0.0907"); + Serial.println(QuaternionUtils::quatConjugate(random_quat)); + + BLA::Matrix<3, 1> v1_b = {-8.4870, -4.7330, -1.2682}; + BLA::Matrix<3, 1> v2_b = {-0.1710, 0.0429, 0.9843}; + BLA::Matrix<3, 1> v1_i = {0, 0, 9.8}; + BLA::Matrix<3, 1> v2_i = {1, 0, 0}; + + Serial.println("Test triad_BE: Test vecs displayed here in code. Expected: \ + -0.1710 -0.4698 -0.8660 \ + 0.0429 0.8746 -0.4830 \ + 0.9843 -0.1197 -0.1294"); + Serial.println(QuaternionUtils::triad_BE(v1_b, v2_b, v1_i, v2_i)); + + +} + +void loop() { +} \ No newline at end of file diff --git a/src/qmekf.cpp b/src/qmekf.cpp new file mode 100644 index 0000000..4f3eddb --- /dev/null +++ b/src/qmekf.cpp @@ -0,0 +1,510 @@ +// #include "qmekf.h" + +// #include "BasicLinearAlgebra.h" + +// #include + +// StateEstimator::StateEstimator(const TimedPointer IMUData, +// const TimedPointer gpsData, +// float dt) : IMUData(IMUData), gpsData(gpsData) { +// P.Fill(0.0f); +// for(uint8_t idx : QMEKFInds::quat) { +// P(idx, idx) = 1e-8; +// } +// for (uint8_t idx : QMEKFInds::vel) { +// P(idx, idx) = 1e-8; +// } +// for (uint8_t idx : QMEKFInds::pos) { +// P(idx, idx) = 1e-8; +// } +// for(uint8_t idx : QMEKFInds::gyroBias) { +// P(idx, idx) = powf(icm20948_const::gyro_VRW, 2.0f); +// } +// for(uint8_t idx: QMEKFInds::accelBias) { +// P(idx, idx) = powf(icm20948_const::accelXY_VRW, 2.0f); +// } +// for(uint8_t idx : QMEKFInds::magBias) { +// // P(idx, idx) = icm20948_const.magXYZ_var; +// P(idx, idx) = powf(0.1f, 2); +// } +// for (uint8_t idx : QMEKFInds::baroBias) { +// P(idx, idx) = powf(0.1f, 2); +// } + +// P_min = P; + + +// //ToDo: Look at this alter, big ass matrix or integration +// Q.Fill(0.0f); +// for(uint8_t idx : QMEKFInds::quat) { +// Q(idx, idx) = 1e-8; +// } +// for(uint8_t idx : QMEKFInds::vel) { +// Q(idx, idx) = 1e-8; +// } +// for(uint8_t idx : QMEKFInds::pos) { +// Q(idx, idx) = 1e-8; +// } +// for(uint8_t idx : QMEKFInds::gyroBias) { +// Q(idx, idx) = powf(0.1f,2); +// } +// for(uint8_t idx : QMEKFInds::accelBias) { +// Q(idx, idx) = powf(0.01f,2); +// } +// for(uint8_t idx : QMEKFInds::magBias) { +// Q(idx, idx) = powf(0.1f, 2); +// } +// for(uint8_t idx : QMEKFInds::baroBias) { +// Q(idx, idx) = powf(0.1f, 2); +// } + +// #ifdef DBG +// Serial.println("<----- Process Noise ----->"); +// for (int i = 0; i < Q.Rows; i++) { +// for (int j = 0; j < Q.Cols; j++) { +// Serial.print(String(Q(i,j)) + "\t"); +// } +// Serial.println(""); +// } + +// Serial.println("<----- Initial Error Cov ----->"); +// for (int i = 0; i < P.Rows; i++) { +// for (int j = 0; j < P.Cols; j++) { +// Serial.print(String(P(i,j)) + "\t"); +// } +// Serial.println(""); +// } +// #endif + +// } +// void StateEstimator::init(BLA::Matrix<3, 1> LLA){ +// // Accelerometer +// BLA::Matrix<3,1> a_b = { +// IMUData->accelX, +// IMUData->accelY, +// IMUData->accelZ +// }; +// a_b = a_b / BLA::Norm(a_b); + +// // Flip gravity direction: ensure z points DOWN in body frame +// float ax = a_b(0), ay = a_b(1), az = a_b(2); + +// // Compute roll (phi) and pitch (theta) assuming NED frame +// float roll = atan2(-ay, -az); // Flip signs to match z-down NED +// float pitch = atan2(ax, sqrt(ay*ay + az*az)); + +// // Magnetometer +// BLA::Matrix<3,1> m_b = { +// IMUData->magX, +// IMUData->magY, +// IMUData->magZ +// }; + +// // Tilt compensation +// float mx = m_b(0), my = m_b(1), mz = m_b(2); + +// float mx2 = mx * cos(pitch) + mz * sin(pitch); +// float my2 = mx * sin(roll) * sin(pitch) + my * cos(roll) - mz * sin(roll) * cos(pitch); + +// float yaw = atan2(-my2, mx2); // Heading angle (NED convention) + +// // Convert roll/pitch/yaw to quaternion (ZYX convention) +// float cy = cos(yaw * 0.5f); +// float sy = sin(yaw * 0.5f); +// float cp = cos(pitch * 0.5f); +// float sp = sin(pitch * 0.5f); +// float cr = cos(roll * 0.5f); +// float sr = sin(roll * 0.5f); + +// BLA::Matrix<4,1> q0 = { +// cr * cp * cy + sr * sp * sy, // w +// sr * cp * cy - cr * sp * sy, // x +// cr * sp * cy + sr * cp * sy, // y +// cr * cp * sy - sr * sp * cy // z +// }; + +// this->x = {q0(0), q0(1), q0(2), q0(3), +// 0, 0, 0, // velocity +// 0, 0, 0, // position +// 0, 0, 0, // gyro bias +// 0, 0, 0, // accel bias +// 0, 0, 0, // mag bias +// 0}; // baro bias + +// this->x_min = x; + +// // Set launch site LLA/ECEF +// launch_ecef = QuaternionUtils::lla2ecef(LLA); +// launch_lla = LLA; +// BLA::Matrix<2, 1> ll = {launch_lla(0), launch_lla(1)}; +// R_ET = QuaternionUtils::dcm_ned2ecef(ll); + +// lastTimes = {millis(), millis(), millis(), millis(), millis()}; +// } + + +// BLA::Matrix<20,1> StateEstimator::onLoop(int state) { +// // Read data from sensors and convert values + +// float dt; + +// float gyrX = IMUData->gyrX; +// float gyrY = IMUData->gyrY; +// float gyrZ = IMUData->gyrZ; + +// float aclX = IMUData->accelX; +// float aclY = IMUData->accelY; +// float aclZ = IMUData->accelZ; + +// float magX = IMUData->magX; +// float magY = IMUData->magY; +// float magZ = IMUData->magZ; + + +// // TODO get in the GPS data and baro data from somewhere +// BLA::Matrix<3,1> gyro = {gyrX, gyrY, gyrZ}; // [rad/s] +// BLA::Matrix<3,1> accel = {aclX, aclY, aclZ}; // [m/s^s] +// BLA::Matrix<3,1> mag = {magX, magY, magZ}; // [uT] +// BLA::Matrix<3,1> gpsData = {gpsData(0), gpsData(1), gpsData(2)}; +// // BLA::Matrix<1,1> baro = {baroZ}; + +// // Remove biases from each measurement +// BLA::Matrix<3,1> unbiased_gyro = {gyro(0) - x(QMEKFInds::gb_x), gyro(1) - x(QMEKFInds::gb_y), gyro(2) - x(QMEKFInds::gb_z)}; +// BLA::Matrix<3,1> unbiased_accel = {accel(0) - x(QMEKFInds::ab_x), accel(1) - x(QMEKFInds::ab_y), accel(2) - x(QMEKFInds::ab_z)}; +// BLA::Matrix<3,1> unbiased_mag = {mag(0) - x(QMEKFInds::mb_x), mag(1) - x(QMEKFInds::mb_y), mag(2) - x(QMEKFInds::mb_z)}; +// // BLA::Matrix<1,1> unbiased_baro = {baro(0) - x(20)}; + + +// float time = millis(); +// bool run_priori = time - lastTimes(0) >= frequencies(0); +// bool run_accel_update = time - lastTimes(1) >= frequencies(1); +// bool run_mag_update = time - lastTimes(2) >= frequencies(2); +// bool run_gps_update = time - lastTimes(3) >= frequencies(3); +// // bool run_baro_update = time - lastTimeBaro(4) >= frequencies(4); + + +// if(run_priori) { +// // TODO eventually implement RK4 here, but I don't understand it yet +// float lastAttUpdate = max(max(lastTimes(0), lastTimes(1)), lastTimes(2)); // Maximum of the lastTimes(0, 1, 2) +// float lastPVUpdate = max(max(lastTimes(0), lastTimes(3)), lastTimes(4)); // Maximum of the lastTimes(0, 3, 4) +// float dt_att = millis() - lastAttUpdate; +// float dt_pv = millis() - lastPVUpdate; + +// x = fastIMUProp(gyro, accel, dt_att, dt_pv); + + +// lastTimes(0) = millis(); +// } + +// if (run_accel_update || run_mag_update || run_gps_update) { +// dt = millis() - max(max(lastTimes(0), lastTimes(1)), max(lastTimes(2), lastTimes(3)));; +// P = predictionFunction(P, gyro, accel, dt); +// } + +// if (run_accel_update) { +// runAccelUpdate(x, accel); +// lastTimes(1) = millis(); +// } + +// if (run_mag_update) { +// runMagUpdate(x, mag); +// lastTimes(2) = millis(); +// } + +// if (run_gps_update) { +// BLA::Matrix<3,1> gps_ecef = QuaternionUtils::lla2ecef(gpsData); +// runGPSUpdate(x, gps_ecef); +// lastTimes(3) = millis(); +// } + +// // if (run_baro_update) { +// // run_baro_update(); +// // lastTimes(4) = millis(); +// // } + +// //Update sensor readings +// gyro_prev = unbiased_gyro; +// accel_prev = unbiased_accel; +// mag_prev = unbiased_mag; +// gps_prev = gpsData; +// // baro_prev = unbiased_baro; + +// return x; +// } + +// BLA::Matrix<20,1> StateEstimator::fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt) { + +// BLA::Matrix<3, 1> v; +// BLA::Matrix<3, 1> p; + +// // TODO change to from world model when go to ECEF +// BLA::Matrix<3,1> gyro_int = {gyro(0)*att_dt, gyro(1)*att_dt, gyro(2)*att_dt}; +// float rotVecNorm = BLA::Norm(gyro_int); +// BLA::Matrix<3,1> axis = {(gyro_int(0) / rotVecNorm), (gyro_int(1) / rotVecNorm), (gyro_int(2) / rotVecNorm)}; +// BLA::Matrix<4,1> dq = +// { +// cos(rotVecNorm/2.0f), +// axis(0) * sinf(rotVecNorm/2.0f), +// axis(1) * sinf(rotVecNorm/2.0f), +// axis(2) * sinf(rotVecNorm/2.0f), +// }; +// BLA::Matrix<4,1> q = +// { +// x(QMEKFInds::q_w), +// x(QMEKFInds::q_x), +// x(QMEKFInds::q_y), +// x(QMEKFInds::q_z), +// }; +// q = QuaternionUtils::quatMultiply(q, dq); +// BLA::Matrix<4,1> qNorm = {q(0) / BLA::Norm(q), q(1) / BLA::Norm(q), q(2) / BLA::Norm(q), q(3) / BLA::Norm(q)}; + + + +// BLA::Matrix<3,1> v_dot = QuaternionUtils::quatToRot(q) * accel + g_i; +// BLA::Matrix<3,1> old_v = { +// x(QMEKFInds::v_x), x(QMEKFInds::v_y), x(QMEKFInds::v_z) +// }; +// BLA::Matrix<3,1> old_p = { +// x(QMEKFInds::p_x), x(QMEKFInds::p_y), x(QMEKFInds::p_z) +// }; + +// v = old_v + v_dot * pv_dt; +// p = old_p + v * pv_dt; + +// x(QMEKFInds::v_x) = v(0); +// x(QMEKFInds::v_y) = v(1); +// x(QMEKFInds::v_z) = v(2); +// x(QMEKFInds::p_x) = p(0); +// x(QMEKFInds::p_y) = p(1); +// x(QMEKFInds::p_z) = p(2); + +// return x; + +// } + +// BLA::Matrix<19, 19> StateEstimator::predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt) { +// BLA::Matrix<3,3> gyroSkew = QuaternionUtils::skewSymmetric(gyroVec); +// BLA::Matrix<3,3> accelSkew = QuaternionUtils::skewSymmetric(accelVec); + +// BLA::Matrix<4, 1> q = +// { +// x(QMEKFInds::q_w), +// x(QMEKFInds::q_x), +// x(QMEKFInds::q_y), +// x(QMEKFInds::q_z) +// }; + +// BLA::Matrix<3, 3> rotMatrix = QuaternionUtils::quatToRot(q); + +// BLA::Matrix<19, 19> F; +// F.Fill(0); + +// //Row 1 - 3 +// F.Submatrix<3, 3>(0, QMEKFInds::q_w) = -1.0f * gyroSkew; +// F.Submatrix<3, 3>(0, QMEKFInds::gb_x) = -1.0f * I_3; + +// //Row 4 - 6 +// F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::q_w) = -1.0f * rotMatrix * accelSkew; +// F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::ab_x) = -1.0f * rotMatrix; + +// //Row 7 - 9 +// F.Submatrix<3, 3>(QMEKFInds::p_x -1, 3) = I_3; + +// BLA::Matrix<19, 19> phi; +// phi.Fill(0); + +// phi = I_19 + (F * dt) + (0.5f * F * F * float(pow(dt, 2))); + +// BLA::Matrix<19, 19> phi_t = ~phi; + +// BLA::Matrix<19, 19> Q_d; +// Q_d.Fill(0); + +// BLA::Matrix<3, 3> gyro_var_diag; +// gyro_var_diag.Fill(0); +// gyro_var_diag(0, 0) = QMEKFInds::gyro_var; +// gyro_var_diag(1, 1) = QMEKFInds::gyro_var; +// gyro_var_diag(2, 2) = QMEKFInds::gyro_var; + +// BLA::Matrix<3, 3> gyro_bias_var_diag; +// gyro_bias_var_diag.Fill(0); +// gyro_bias_var_diag(0, 0) = QMEKFInds::gyro_bias_var; +// gyro_bias_var_diag(1, 1) = QMEKFInds::gyro_bias_var; +// gyro_bias_var_diag(2, 2) = QMEKFInds::gyro_bias_var; + +// BLA::Matrix<3, 3> accel_bias_var_diag; +// accel_bias_var_diag.Fill(0); +// accel_bias_var_diag(0, 0) = QMEKFInds::accel_bias_var; +// accel_bias_var_diag(1, 1) = QMEKFInds::accel_bias_var; +// accel_bias_var_diag(2, 2) = QMEKFInds::accel_bias_var; + +// Q_d.Submatrix<3, 3>(QMEKFInds::q_w, QMEKFInds::q_w) = (gyro_var_diag * dt) + (gyro_bias_var_diag * float((pow(dt, 3) / 10))); +// Q_d.Submatrix<3, 3>(QMEKFInds::q_w, 9) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2)); + +// BLA::Matrix<3, 3> R_grav_diag; +// R_grav_diag.Fill(0); +// R_grav_diag(0, 0) = QMEKFInds::R_grav; +// R_grav_diag(1, 1) = QMEKFInds::R_grav; +// R_grav_diag(2, 2) = QMEKFInds::R_grav; + +// Q_d.Submatrix<3 ,3>(3, 3) = R_grav_diag * dt + accel_bias_var_diag * float((pow(dt, 3) / 3)); +// Q_d.Submatrix<3, 3>(3, 6) = accel_bias_var_diag * float((pow(dt ,4) / 8.0)) + R_grav_diag * float((pow(dt, 2) / 2.0)); +// Q_d.Submatrix<3, 3>(3, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); + +// Q_d.Submatrix<3, 3>(6, 3) = R_grav_diag * float((pow(dt, 2) / 2)) + accel_bias_var_diag * float((pow(dt, 4) / 8.0)); +// Q_d.Submatrix<3, 3>(6, 6) = R_grav_diag * float((pow(dt, 3) / 3.0)) + accel_bias_var_diag * float((pow(dt, 5) / 20.0)); +// Q_d.Submatrix<3, 3>(6, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 3) / 6.0)); + +// Q_d.Submatrix<3, 3>(9, 0) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); +// Q_d.Submatrix<3, 3>(9, 9) = gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); + +// Q_d.Submatrix<3, 3>(12, 3) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); +// Q_d.Submatrix<3, 3>(12, 6) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); +// Q_d.Submatrix<3, 3>(12, 12) = accel_bias_var_diag * dt; + +// Q_d(15, 15) = QMEKFInds::mag_bias_var * dt; + +// Q_d(18, 18) = QMEKFInds::baro_bias_var * dt; + +// BLA::Matrix<19, 19> P; + +// P = phi * P_ * phi_t + Q_d; + +// return P; + +// } + +// BLA::Matrix<20, 1> StateEstimator::runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3,1> accel_meas) +// { +// BLA::Matrix<4,1> q = +// { +// x(QMEKFInds::q_w), +// x(QMEKFInds::q_x), +// x(QMEKFInds::q_y), +// x(QMEKFInds::q_z), +// }; + +// BLA::Matrix<3, 19> H_accel; +// H_accel.Fill(0); +// H_accel.Submatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * (-1.0f * normal_i)); +// H_accel.Submatrix<3, 3>(0, QMEKFInds::ab_x - 1) = -1.0f * QuaternionUtils::quat2DCM(q); + +// BLA::Matrix<3, 1> h_accel; + +// h_accel = QuaternionUtils::quat2DCM(q) * normal_i; + +// EKFCalcErrorInject(x, P, accel_meas, H_accel, h_accel, R_accel); + +// } + +// BLA::Matrix<20, 1> StateEstimator::runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> mag_meas) { +// // TODO input igrm model somehow figure out +// BLA::Matrix<4, 1> q = +// { +// x(QMEKFInds::q_w), +// x(QMEKFInds::q_x), +// x(QMEKFInds::q_y), +// x(QMEKFInds::q_z) +// }; + +// BLA::Matrix<3, 19> H_mag; +// H_mag.Fill(0); +// H_mag.Submatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * m_i); +// H_mag.Submatrix<3, 3>(0, QMEKFInds::gb_x) = I_3; + +// BLA::Matrix<3, 1> h_mag; + +// h_mag = QuaternionUtils::quat2DCM(q) * m_i; + +// EKFCalcErrorInject(x, P, mag_meas, H_mag, h_mag, R_mag); + +// } + +// BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps_meas_ecef) { +// BLA::Matrix<3, 1> pos_ned = QuaternionUtils::ecef2ned(gps_meas_ecef, launch_ecef, R_ET); + +// BLA::Matrix<3, 19> H_gps; +// H_gps.Fill(0); +// H_gps.Submatrix<3, 3>(0, QMEKFInds::p_x) = I_3; + +// BLA::Matrix<3, 1> h_gps = { +// x(QMEKFInds::p_x), +// x(QMEKFInds::p_y), +// x(QMEKFInds::p_z), +// }; + +// EKFCalcErrorInject(x, P, pos_ned, H_gps, h_gps, R_gps); + +// } + +// BLA::Matrix<20, 1> StateEstimator::EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R) { +// BLA::Matrix<3, 1> residual; +// residual = sens_reading - h; + +// BLA::Matrix<3, 3> S; +// BLA::Matrix<19, 3> K; +// BLA::Matrix<19, 3> H_t = ~H; + +// S = H * oldP * H_t + R; +// K = (oldP * H_t) * BLA::Inverse(S); +// BLA::Matrix<19, 1> postErrorState = K * residual; + +// // Inject error angles into quat +// BLA::Matrix<3, 1> alpha; +// alpha = {postErrorState(0), postErrorState(1), postErrorState(2)}; +// BLA::Matrix<3, 1> rotVec = 1.0f * alpha; +// float rotVecNorm = BLA::Norm(rotVec); +// BLA::Matrix<3,1> axis = rotVec / rotVecNorm; +// BLA::Matrix<4,1> dq = +// { +// cos(rotVecNorm/2.0f), +// axis(0) * sinf(rotVecNorm/2.0f), +// axis(1) * sinf(rotVecNorm/2.0f), +// axis(2) * sinf(rotVecNorm/2.0f), +// }; +// BLA::Matrix<4, 1> old_q = +// { +// x(0), +// x(1), +// x(2), +// x(3) +// }; +// BLA::Matrix<4, 1> q = QuaternionUtils::quatMultiply(old_q, dq); + +// // Set quats +// x(0) = q(0); +// x(1) = q(1); +// x(2) = q(2); +// x(3) = q(3); + +// // Set velocity +// x(4) = x(4) + postErrorState(3); +// x(5) = x(5) + postErrorState(4); +// x(6) = x(6) + postErrorState(5); + +// // Set position +// x(7) = x(7) + postErrorState(6); +// x(8) = x(8) + postErrorState(7); +// x(9) = x(9) + postErrorState(8); + +// // Set gyro bias +// x(10) = x(10) + postErrorState(9); +// x(11) = x(11) + postErrorState(10); +// x(12) = x(12) + postErrorState(11); + +// // Set accel bias +// x(13) = x(13) + postErrorState(12); +// x(14) = x(14) + postErrorState(13); +// x(15) = x(15) + postErrorState(14); + +// // Set mag bias +// x(16) = x(16) + postErrorState(15); +// x(17) = x(17) + postErrorState(16); +// x(18) = x(18) + postErrorState(17); + +// // Set baro bias +// x(19) = x(19) + postErrorState(18); + +// return x; +// } + diff --git a/src/qmekf.h b/src/qmekf.h new file mode 100644 index 0000000..6a299cb --- /dev/null +++ b/src/qmekf.h @@ -0,0 +1,190 @@ +// #pragma once + +// #include "BasicLinearAlgebra.h" + +// #include "kfConsts.h" +// #include +// #include "QuaternionUtils.h" + +// /** +// * @name QMEKfInds +// * @brief Struct holding the indices of the total QMEKF +// */ +// namespace QMEKFInds { +// constexpr uint8_t q_w = 0; +// constexpr uint8_t q_x = 1; +// constexpr uint8_t q_y = 2; +// constexpr uint8_t q_z = 3; +// constexpr std::array quat = {q_w, q_x, q_y, q_z}; + +// constexpr uint8_t v_x = 4; +// constexpr uint8_t v_y = 5; +// constexpr uint8_t v_z = 6; +// constexpr std::array vel = {v_x, v_y, v_z}; + +// constexpr uint8_t p_x = 7; +// constexpr uint8_t p_y = 8; +// constexpr uint8_t p_z = 9; +// constexpr std::array pos = {p_x, p_y, p_z}; + +// constexpr uint8_t gb_x = 10; +// constexpr uint8_t gb_y = 11; +// constexpr uint8_t gb_z = 12; +// constexpr std::array gyroBias = {gb_x, gb_y, gb_z}; + +// constexpr uint8_t ab_x = 13; +// constexpr uint8_t ab_y = 14; +// constexpr uint8_t ab_z = 15; +// constexpr std::array accelBias = {ab_x, ab_y, ab_z}; + +// constexpr uint8_t mb_x = 16; +// constexpr uint8_t mb_y = 17; +// constexpr uint8_t mb_z = 18; +// constexpr std::array magBias = {mb_x, mb_y, mb_z}; + +// constexpr uint8_t bb_z = 19; +// constexpr std::array baroBias = {bb_z}; + + + +// }; // namespace QMEKFInds + +// #define QMEKF_LOG_DESC(X) \ +// X(0, "w", p.print(state(QMEKFInds::q_w), 4)) \ +// X(1, "i", p.print(state(QMEKFInds::q_x), 4)) \ +// X(2, "j", p.print(state(QMEKFInds::q_y), 4)) \ +// X(3, "k", p.print(state(QMEKFInds::q_z), 4)) \ +// X(4, "v_x", p.print(state(QMEKFInds::v_x), 4)) \ +// X(5, "v_y", p.print(state(QMEKFInds::v_y), 4)) \ +// X(6, "v_z", p.print(state(QMEKFInds::v_z), 4)) \ +// X(7, "p_x", p.print(state(QMEKFInds::p_x), 4)) \ +// X(8, "p_y", p.print(state(QMEKFInds::p_y), 4)) \ +// X(9, "p_z", p.print(state(QMEKFInds::p_z), 4)) \ + + +// /** +// * @name QMEKFStateEstimator +// * @author QMEKF team +// * @brief Attitude and Position/Velocity estimation. See matlab simulation for details +// */ +// class StateEstimator { + +// public: +// StateEstimator(const TimedPointer, const TimedPointer, float dt); + +// /** +// * @name init +// * @author @frostydev99 +// * @param x_0 - Initial State +// * @param dt - Discrete time step +// */ +// void init(BLA::Matrix<3, 1> LLA); + +// /** +// * @name onLoop +// * @author @frostydev99 +// * @brief Run Every Loop +// * @paragraph This method should run every loop of the expected prediction +// * update rate given by dt +// */ +// BLA::Matrix<20, 1> onLoop(int state); + +// private: +// const TimedPointer IMUData; +// const TimedPointer gpsData; + +// // Prediction Functions +// // BLA::Matrix<20, 1> predictionFunction(BLA::Matrix<20, 1> x, +// // BLA::Matrix<3, 1> gyro, +// // BLA::Matrix<3, 1> accel); +// // BLA::Matrix<19, 19> predictionJacobian(BLA::Matrix<20, 1> x, +// // BLA::Matrix<3, 1> gyro, +// // BLA::Matrix<3, 1> accel); + +// BLA::Matrix<20,1> fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt); +// BLA::Matrix<19, 19> predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt); + +// // Update Functions +// BLA::Matrix<20,1> runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> a_b); + +// BLA::Matrix<20,1> runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> m_b); + +// BLA::Matrix<20,1> runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps); + +// // void runBaroUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<1, 1> baro); + +// BLA::Matrix<20,1> EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R); + +// // State Vector Allocation +// BLA::Matrix<20, 1> x; + +// // Error Covariance Allocation +// BLA::Matrix<19, 19> P; + +// // Identity Matrices +// BLA::Matrix<20, 20> I_20 = BLA::Eye<20, 20>(); +// BLA::Matrix<19, 19> I_19 = BLA::Eye<19, 19>(); +// BLA::Matrix<3, 3> I_3 = BLA::Eye<3, 3>(); + +// //R values +// float accel_var = pow(sqrt(asm330_const::accelXY_var) * 9.8, 2); +// float mag_var = icm20948_const::magXYZ_var; +// float gps_var = Max10S_const::gpsXYZ_var; +// //R matricies +// BLA::Matrix<3, 3> R_accel = {accel_var, 0, 0, +// 0, accel_var, 0, +// 0, 0, accel_var}; + +// BLA::Matrix<3, 3> R_mag = {mag_var, 0, 0, +// 0, mag_var, 0, +// 0, 0, mag_var}; + +// BLA::Matrix<3, 3> R_gps= {gps_var, 0, 0, +// 0, gps_var, 0, +// 0, 0, gps_var}; + +// BLA::Matrix<10, 1> R_all = { +// pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), +// pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), +// pow(sqrt(asm330_const::accelZ_var) * 9.8, 2), +// icm20948_const::magXYZ_var, +// icm20948_const::magXYZ_var, +// icm20948_const::magXYZ_var, +// Max10S_const::gpsXYZ_var, +// Max10S_const::gpsXYZ_var, +// Max10S_const::gpsXYZ_var, +// Max10S_const::baro_var +// }; + + +// BLA::Matrix<5, 1> lastTimes = {0, 0, 0, 0, 0, 0, 0}; +// // Gyro prop, accel prop, accel update, mag update, gps update, baro update + +// BLA::Matrix<3,1> gyro_prev = {0, 0, 0}; +// BLA::Matrix<3,1> accel_prev = {0, 0, 0}; +// BLA::Matrix<3,1> mag_prev = {0, 0, 0}; +// BLA::Matrix<3,1> gps_pos_prev = {0, 0, 0}; +// BLA::Matrix<3,1> gps_vel_prev = {0, 0, 0}; +// BLA::Matrix<1,1> baro_prev = {0}; + +// BLA::Matrix<3, 1> normal_i = {0, 0, -9.8037}; // [m/s^2] +// BLA::Matrix<3, 1> g_i = {0, 0, 9.8037}; // [m/s^2] +// BLA::Matrix<3, 1> m_i = {18.659605, -4.540227, 49.09786}; // [uT] Kids rocket params +// BLA::Matrix<3, 1> launch_ecef = {1311800, -4337300, 4473600}; // [m] // asuming 0 above surface +// BLA::Matrix<3, 1> launch_lla = {44.825070, -73.171726, 0}; // [whatever tf units are in (deg, deg, m)] +// BLA::Matrix<2, 1> launch_ll = {launch_lla(0), launch_lla(1)}; +// BLA::Matrix<3, 3> R_ET = QuaternionUtils::dcm_ned2ecef(launch_ll); + +// template +// BLA::Matrix extractSub(const BLA::Matrix &x, +// const std::array &inds) { +// BLA::Matrix sub; +// for (int i = 0; i < M; i++) { +// sub(i) = x(inds[i]); +// } +// return sub; +// } + +// BLA::Matrix<3, 3> quat2rot(const BLA::Matrix<4, 1> &q); + +// }; diff --git a/test/README b/test/README new file mode 100644 index 0000000..9b1e87b --- /dev/null +++ b/test/README @@ -0,0 +1,11 @@ + +This directory is intended for PlatformIO Test Runner and project tests. + +Unit Testing is a software testing method by which individual units of +source code, sets of one or more MCU program modules together with associated +control data, usage procedures, and operating procedures, are tested to +determine whether they are fit for use. Unit testing finds problems early +in the development cycle. + +More information about PlatformIO Unit Testing: +- https://docs.platformio.org/en/latest/advanced/unit-testing/index.html From f034a9e31e179085e19b2fcfa4a5ea7eb24d6fc1 Mon Sep 17 00:00:00 2001 From: Abhay Mathur <66543532+abhayma1000@users.noreply.github.com> Date: Wed, 3 Dec 2025 16:59:45 -0500 Subject: [PATCH 05/14] Initialization and book keeping --- platformio.ini | 16 +- src/QuaternionUtils.cpp | 68 +++++++- src/QuaternionUtils.h | 33 +++- src/kfConsts.h | 32 ++-- src/main.cpp | 98 +++++++----- src/qmekf.cpp | 228 +++++++++++--------------- src/qmekf.h | 345 ++++++++++++++++++---------------------- 7 files changed, 420 insertions(+), 400 deletions(-) diff --git a/platformio.ini b/platformio.ini index 23d465b..737b3c5 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,23 +8,13 @@ ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html -[env:disco_g031j6] +[env:nucleo_h753zi] platform = ststm32 -board = disco_g031j6 +board = nucleo_h753zi framework = arduino - -lib_deps = - adafruit/Adafruit LPS2X@^2.0.6 - adafruit/Adafruit BusIO@^1.17.0 - adafruit/Adafruit Unified Sensor@^1.1.15 - stm32duino/STM32duino ASM330LHH@^2.0.1 - adafruit/Adafruit ICM20X@^2.0.7 - sparkfun/SparkFun u-blox GNSS v3@^3.1.8 - greiman/SdFat - nanopb/Nanopb@^0.4.91 +lib_deps = tomstewart89/BasicLinearAlgebra @ ^5.1 Wire SPI Servo IWatchdog - diff --git a/src/QuaternionUtils.cpp b/src/QuaternionUtils.cpp index e948398..1f5d28c 100644 --- a/src/QuaternionUtils.cpp +++ b/src/QuaternionUtils.cpp @@ -1,6 +1,7 @@ #include "QuaternionUtils.h" #include "BasicLinearAlgebra.h" #include +#include BLA::Matrix<3, 3> QuaternionUtils::quatToRot(const BLA::Matrix<4, 1> &quat) { float w = quat(0); @@ -205,8 +206,10 @@ BLA::Matrix<3, 3> QuaternionUtils::vecs2mat(const BLA::Matrix<3, 1> v1, const BL return ret; } -// 1 is important, 2 is secondary + + BLA::Matrix<4, 1> QuaternionUtils::triad_BE(const BLA::Matrix<3, 1> v1_b, const BLA::Matrix<3, 1> v2_b, const BLA::Matrix<3, 1> v1_i, const BLA::Matrix<3, 1> v2_i) { + // 1 is important, 2 is secondary BLA::Matrix<3, 1> v1_b_norm = v1_b / BLA::Norm(v1_b); BLA::Matrix<3, 1> v2_b_norm = v1_b / BLA::Norm(v2_b); BLA::Matrix<3, 1> v1_i_norm = v1_b / BLA::Norm(v1_i); @@ -231,8 +234,71 @@ BLA::Matrix<4, 1> QuaternionUtils::triad_BE(const BLA::Matrix<3, 1> v1_b, const return dcm2quat(R_BE); +} + +BLA::Matrix<3,1> QuaternionUtils::lla2ecef(BLA::Matrix<3,1> lla) { + float pi = 3.141592653589; // TODO move this somewhere better (idk c++) + float a = 6378137.0; // WGS-84 semi-major axis + float f = 1.0 / 298.257223563; // flattening + float e2 = f * (2.0f - f); // eccentricity squared + + float lat = lla(0) * pi / 180.0; // Convert to radians + float lon = lla(1) * pi / 180.0; // Convert to radians + float alt = lla(2); + + // Convert reference lla to ecef first + float N = a / std::sqrt(1 - e2 * std::pow(std::sin(lat), 2)); + float x = (N + alt) * std::cos(lat) * std::cos(lon); + float y = (N + alt) * std::cos(lat) * std::sin(lon); + float z = ((1 - e2) * N + alt) * std::sin(lat); + BLA::Matrix<3,1> ecef_coords = {x,y,z}; + return ecef_coords; + +} + +BLA::Matrix<3, 1> QuaternionUtils::ecef2lla(BLA::Matrix<3, 1> ecef) { + // Follows this kid idk if right, gotta test, but I'm not writing this bs myself + // I think this is's impl not super precise, but not that deep (I think) + // https://github.com/kvenkman/ecef2lla/blob/master/ecef2lla.py + + float pi = 3.141592653589; + float a = 6378137.0f; + float a_sq = a * a; + float e = 8.181919084261345e-2; + float e_sq = 6.69437999014e-3; + + float f = 1.0f/298.257223563; + float b = a*(1.0-f); + + float r = std::sqrt(ecef(0) * ecef(0) + ecef(1) * ecef(1)); + float ep_sq = (a * a - b * b) / (b * b); + float ee = (a * a - b * b); + f = (54 * b * b) * (ecef(2) * ecef(2)); + float g = r * r + (1 - e_sq) * (ecef(2) * ecef(2)) - e_sq * ee * 2.0f; + float c = (e_sq * e_sq) * f * r * r / (g * g * g); + float s = std::pow((1.0 + c + std::sqrt(c * c + 2 * c)), (1.0f / 3.0f)); + float p = f / (3.0f * (g * g) * std::pow(s + (1.0 / s) + 1.0, 2.0f)); + float q = std::sqrt(1.0f + 2.0f * p * std::pow(e_sq, 2.0f)); + // float r_0 = + // TODO got lazy, gotta finish this tho + // r_0 = -(p*e_sq*r)/(1+q) + np.sqrt(0.5*(a**2)*(1+(1./q)) - p*(z**2)*(1-e_sq)/(q*(1+q)) - 0.5*p*(r**2)) + // u = np.sqrt((r - e_sq*r_0)**2 + z**2) + // v = np.sqrt((r - e_sq*r_0)**2 + (1 - e_sq)*z**2) + // z_0 = (b**2)*z/(a*v) + // h = u*(1 - b**2/(a*v)) + // phi = np.arctan((z + ep_sq*z_0)/r) + // lambd = np.arctan2(y, x) + + float phi = 0.0f; + float lambd = 0.0f; + float h = 0.0f; + BLA::Matrix<3, 1> lla = {phi*180.0f/pi, lambd*180.0/pi, h}; + return lla; } + + + diff --git a/src/QuaternionUtils.h b/src/QuaternionUtils.h index 564cd2a..eb8892a 100644 --- a/src/QuaternionUtils.h +++ b/src/QuaternionUtils.h @@ -55,5 +55,36 @@ namespace QuaternionUtils { BLA::Matrix<4, 1> triad_BE(const BLA::Matrix<3, 1> v1_b, const BLA::Matrix<3, 1> v2_b, const BLA::Matrix<3, 1> v1_i, const BLA::Matrix<3, 1> v2_i); - + BLA::Matrix<3, 1> lla2ecef(BLA::Matrix<3,1> lla); + + BLA::Matrix<3, 1> ecef2lla(BLA::Matrix<3, 1> ecef); + + template + BLA::Matrix extractSub(const BLA::Matrix &x, + const std::array &inds) { + BLA::Matrix sub; + for (int i = 0; i < M; i++) { + sub(i) = x(inds[i]); + } + return sub; + } + + template + BLA::Matrix extractDiag(const BLA::Matrix &x) { + BLA::Matrix diag; + for (int i = 0; i < M; i++) { + diag(i) = x(inds[i], indx[i]); + } + return diag; + } + + template + float vecMax(const BLA::Matrix &x) { + BLA::Matrix diag; + for (int i = 0; i < M; i++) { + diag(i) = x(inds[i], indx[i]); + } + return diag; + // TODO fix + } } diff --git a/src/kfConsts.h b/src/kfConsts.h index 4bb84b3..c51455d 100644 --- a/src/kfConsts.h +++ b/src/kfConsts.h @@ -2,8 +2,8 @@ #include "BasicLinearAlgebra.h" #include -constexpr static float g = 9.80665; // [m/s/s] Earth's Grav Accel - +constexpr static float g = 9.80665; // [m/s/s] Earth's Grav Accel TODO eventually take out when have wgs model +// TODO: Fill out this file with correct terms and our units constexpr float square(float a) { return a * a; } namespace asm330_const { @@ -15,10 +15,15 @@ constexpr float gyroX_var = square(0.0947f); // [deg/s] constexpr float gyroY_var = square(0.1474f); // [deg/s] constexpr float gyroZ_var = square(0.2144f); // [deg/s] constexpr float gyro_VRW = 0.0241f; // [deg/s/sqrt(Hz)] + + +BLA::Matrix<3, 1> accel_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; // TODO these are in g's, mode to m/s^2 +BLA::Matrix<3, 1> gyro_var = {square(0.0947f), square(0.0947f), square(0.0947f)}; // TODO these are in deg, mode to rads }; // namespace asm330_const namespace lps22_const { -constexpr float baro_var = 0.0f; +BLA::Matrix<1, 1> baro_var = {0.0f}; // uh what, why is it 0 +BLA::Matrix<1, 1> baro_bias = {0.0f}; }; namespace icm20948_const { @@ -32,26 +37,29 @@ constexpr float gyro_VRW = 8.33e-4f; // [rad/s/sqrt(Hz)] constexpr float magXYZ_var = square(0.7263f); // [uT] constexpr float quatVar = 0.3; // Idk Guess +BLA::Matrix<3, 1> accel_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; // TODO these are in g's, mode to m/s^2 +BLA::Matrix<3, 1> gyro_var = {square(0.0947f), square(0.0947f), square(0.0947f)}; // TODO these are in deg, mode to rads +BLA::Matrix<3, 1> mag_var = {1.0f, 2.0f, 3.0f}; // [uT] +BLA::Matrix<3, 1> mag_bias = {0, 0, 0}; }; // namespace icm20948_const namespace Max10S_const { - constexpr float gpsXYZ_var = square(2.0); // [m] idk guess - constexpr float baro_var = square(7.5); // [P] idk guess + BLA::Matrix<3, 1> gpsPos_var = {square(2.0f), square(2.0f), square(2.0f)}; // [m] idk guess + BLA::Matrix<3, 1> gpsVel_var = {square(0.5f), square(0.5f), square(0.5f)}; // [m/s] } namespace vimu_const { // TODO vimu/2imu - constexpr float gyro_var = 0.0051; - constexpr float gyro_bias_var = square(4.9e-5f * 9.8f); - - BLA::Matrix<3, 1> accel_var = {square(4.9e-5f * 9.9f), square(4.9e-5f * 9.9f), square(4.9e-5f * 9.9f)}; - constexpr float accel_bias_var = square(0.00098f * 9.8f); + // TODO implement algorithm here (or don't and just do it in matlab script). For right now, just using ICM values - constexpr float mag_bias_var = 25; - constexpr float baro_bias_var = square(7.5f); + BLA::Matrix<3, 1> accel_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; // TODO these are in g's, mode to m/s^2 + BLA::Matrix<3, 1> gyro_var = {square(0.0947f), square(0.0947f), square(0.0947f)}; // TODO these are in deg, mode to rads + BLA::Matrix<3, 1> mag_var = {1.0f, 2.0f, 3.0f}; + BLA::Matrix<3, 1> accel_bias = {0, 0, 0}; + BLA::Matrix<3, 1> gyro_bias = {0, 0, 0}; } diff --git a/src/main.cpp b/src/main.cpp index 450c236..f029215 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -5,81 +5,91 @@ #include "QuaternionUtils.h" void setup() { - // Since basing this whole c++ impl on the simulink, using that as reference here - Serial.println("Hello"); +// // Since basing this whole c++ impl on the simulink, using that as reference here +// Serial.println("Hello"); - BLA::Matrix<4, 1> random_quat = {0.9344, 0.2435, 0.2435, 0.0907}; - BLA::Matrix<4, 1> random_quat2 = {0.3666, 0.4564, 0.7857, 0.1998}; - BLA::Matrix<3, 3> random_mat = {0.8662, 0.0206,0.2123, 0.6011, 0.9699, 0.1818, 0.7081, 0.8324, 0.1834}; - BLA::Matrix<3, 1> random_vec = {0.3042, 0.5248, 0.4319}; - BLA::Matrix<3, 1> school_lla = {42.274027, -71.811788, 10}; +// BLA::Matrix<4, 1> random_quat = {0.9344, 0.2435, 0.2435, 0.0907}; +// BLA::Matrix<4, 1> random_quat2 = {0.3666, 0.4564, 0.7857, 0.1998}; +// BLA::Matrix<3, 3> random_mat = {0.8662, 0.0206,0.2123, 0.6011, 0.9699, 0.1818, 0.7081, 0.8324, 0.1834}; +// BLA::Matrix<3, 1> random_vec = {0.3042, 0.5248, 0.4319}; +// BLA::Matrix<3, 1> school_lla = {42.274027, -71.811788, 10}; - Serial.println("Test quat2dcm: Quat: {0.9344, 0.2435, 0.2435, 0.0907}. Expected DCM: \ - 0.8650 0.2880 -0.4109 \ - -0.0508 0.8649 0.4993 \ - 0.4992 -0.4110 0.7628. Actual DCM:"); +// Serial.println("Test quat2dcm: Quat: {0.9344, 0.2435, 0.2435, 0.0907}. Expected DCM: \ +// 0.8650 0.2880 -0.4109 \ +// -0.0508 0.8649 0.4993 \ +// 0.4992 -0.4110 0.7628. Actual DCM:"); - Serial.println(QuaternionUtils::quat2DCM(random_quat)); +// Serial.println(QuaternionUtils::quat2DCM(random_quat)); - Serial.println("Test skewSymmetric: Random vec: {0.3042, 0.5248, 0.4319}. Expected Mat: \ - 0 -0.4319 0.5248 \ - 0.4319 0 -0.3042 \ - -0.5248 0.3042 0"); +// Serial.println("Test skewSymmetric: Random vec: {0.3042, 0.5248, 0.4319}. Expected Mat: \ +// 0 -0.4319 0.5248 \ +// 0.4319 0 -0.3042 \ +// -0.5248 0.3042 0"); - Serial.println(QuaternionUtils::skewSymmetric(random_vec)); +// Serial.println(QuaternionUtils::skewSymmetric(random_vec)); - Serial.println("Test rotVec2Quat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {0.9315, 0.2769, 0.4776, 0.3931}"); +// Serial.println("Test rotVec2Quat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {0.9315, 0.2769, 0.4776, 0.3931}"); - Serial.println(QuaternionUtils::rotVec2Quat(random_vec)); +// Serial.println(QuaternionUtils::rotVec2Quat(random_vec)); - Serial.println("Test smallAngleRotVec2Quat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {1.0000, 0.1521, 0.2624, 0.2160}"); +// Serial.println("Test smallAngleRotVec2Quat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {1.0000, 0.1521, 0.2624, 0.2160}"); - Serial.println(QuaternionUtils::smallAngleRotVec2Quat(random_vec)); +// Serial.println(QuaternionUtils::smallAngleRotVec2Quat(random_vec)); -// TODO eventually test dcm2quat +// // TODO eventually test dcm2quat - Serial.println("Test quatMultiply: Random Quats: {0.9344 0.2435 0.2435 0.0907} and {0.3666 0.4564 0.7857 0.1998} \ - Expected Quat: {0.0220 0.4932 0.8162 0.3002}"); +// Serial.println("Test quatMultiply: Random Quats: {0.9344 0.2435 0.2435 0.0907} and {0.3666 0.4564 0.7857 0.1998} \ +// Expected Quat: {0.0220 0.4932 0.8162 0.3002}"); - Serial.println(QuaternionUtils::quatMultiply(random_quat, random_quat2)); +// Serial.println(QuaternionUtils::quatMultiply(random_quat, random_quat2)); - Serial.println("Test lla2ecef: School LLA: {42.274027, -71.811788, 10}. Expected ECEF: {1475354, -4490428, 4268181}"); +// Serial.println("Test lla2ecef: School LLA: {42.274027, -71.811788, 10}. Expected ECEF: {1475354, -4490428, 4268181}"); - Serial.println(QuaternionUtils::lla2ecef(school_lla)); +// Serial.println(QuaternionUtils::lla2ecef(school_lla)); - Serial.println("Test dcm_ned2ecef: School LLA: {42.274027, -71.811788, 10}. Expected DCM: \ - -0.2100 0.9500 -0.2310 \ - 0.6391 0.3121 0.7030 \ - 0.7399 0 -0.6727"); - Serial.println(QuaternionUtils::dcm_ned2ecef(school_lla)); +// Serial.println("Test dcm_ned2ecef: School LLA: {42.274027, -71.811788, 10}. Expected DCM: \ +// -0.2100 0.9500 -0.2310 \ +// 0.6391 0.3121 0.7030 \ +// 0.7399 0 -0.6727"); +// Serial.println(QuaternionUtils::dcm_ned2ecef(school_lla)); - Serial.println("Test ecef2nedVec:"); // Eh one day +// Serial.println("Test ecef2nedVec:"); // Eh one day - Serial.println("Test quatConj: Test quat: {0.9344 0.2435 0.2435 0.0907}. Expected quat: 0.9344 -0.2435 -0.2435 -0.0907"); - Serial.println(QuaternionUtils::quatConjugate(random_quat)); +// Serial.println("Test quatConj: Test quat: {0.9344 0.2435 0.2435 0.0907}. Expected quat: 0.9344 -0.2435 -0.2435 -0.0907"); +// Serial.println(QuaternionUtils::quatConjugate(random_quat)); - BLA::Matrix<3, 1> v1_b = {-8.4870, -4.7330, -1.2682}; - BLA::Matrix<3, 1> v2_b = {-0.1710, 0.0429, 0.9843}; - BLA::Matrix<3, 1> v1_i = {0, 0, 9.8}; - BLA::Matrix<3, 1> v2_i = {1, 0, 0}; +// BLA::Matrix<3, 1> v1_b = {-8.4870, -4.7330, -1.2682}; +// BLA::Matrix<3, 1> v2_b = {-0.1710, 0.0429, 0.9843}; +// BLA::Matrix<3, 1> v1_i = {0, 0, 9.8}; +// BLA::Matrix<3, 1> v2_i = {1, 0, 0}; - Serial.println("Test triad_BE: Test vecs displayed here in code. Expected: \ - -0.1710 -0.4698 -0.8660 \ - 0.0429 0.8746 -0.4830 \ - 0.9843 -0.1197 -0.1294"); - Serial.println(QuaternionUtils::triad_BE(v1_b, v2_b, v1_i, v2_i)); +// Serial.println("Test triad_BE: Test vecs displayed here in code. Expected: \ +// -0.1710 -0.4698 -0.8660 \ +// 0.0429 0.8746 -0.4830 \ +// 0.9843 -0.1197 -0.1294"); +// Serial.println(QuaternionUtils::triad_BE(v1_b, v2_b, v1_i, v2_i)); + + // Serial.println("TODO Test lla2ecef"); + + // Serial.println("TODO test ecef2lla"); + + // Serial.println("Test extractSub"); + // Serial.println(QuaternionUtils::extractSub(school_lla, {0, 2})); + + // Serial.println("Test extractDiag"); + // Serial.println(QuaternionUtils::extractDiag(random_mat)); } diff --git a/src/qmekf.cpp b/src/qmekf.cpp index 4f3eddb..9244921 100644 --- a/src/qmekf.cpp +++ b/src/qmekf.cpp @@ -1,146 +1,96 @@ -// #include "qmekf.h" - -// #include "BasicLinearAlgebra.h" - -// #include - -// StateEstimator::StateEstimator(const TimedPointer IMUData, -// const TimedPointer gpsData, -// float dt) : IMUData(IMUData), gpsData(gpsData) { -// P.Fill(0.0f); -// for(uint8_t idx : QMEKFInds::quat) { -// P(idx, idx) = 1e-8; -// } -// for (uint8_t idx : QMEKFInds::vel) { -// P(idx, idx) = 1e-8; -// } -// for (uint8_t idx : QMEKFInds::pos) { -// P(idx, idx) = 1e-8; -// } -// for(uint8_t idx : QMEKFInds::gyroBias) { -// P(idx, idx) = powf(icm20948_const::gyro_VRW, 2.0f); -// } -// for(uint8_t idx: QMEKFInds::accelBias) { -// P(idx, idx) = powf(icm20948_const::accelXY_VRW, 2.0f); -// } -// for(uint8_t idx : QMEKFInds::magBias) { -// // P(idx, idx) = icm20948_const.magXYZ_var; -// P(idx, idx) = powf(0.1f, 2); -// } -// for (uint8_t idx : QMEKFInds::baroBias) { -// P(idx, idx) = powf(0.1f, 2); -// } - -// P_min = P; - - -// //ToDo: Look at this alter, big ass matrix or integration -// Q.Fill(0.0f); -// for(uint8_t idx : QMEKFInds::quat) { -// Q(idx, idx) = 1e-8; -// } -// for(uint8_t idx : QMEKFInds::vel) { -// Q(idx, idx) = 1e-8; -// } -// for(uint8_t idx : QMEKFInds::pos) { -// Q(idx, idx) = 1e-8; -// } -// for(uint8_t idx : QMEKFInds::gyroBias) { -// Q(idx, idx) = powf(0.1f,2); -// } -// for(uint8_t idx : QMEKFInds::accelBias) { -// Q(idx, idx) = powf(0.01f,2); -// } -// for(uint8_t idx : QMEKFInds::magBias) { -// Q(idx, idx) = powf(0.1f, 2); -// } -// for(uint8_t idx : QMEKFInds::baroBias) { -// Q(idx, idx) = powf(0.1f, 2); -// } - -// #ifdef DBG -// Serial.println("<----- Process Noise ----->"); -// for (int i = 0; i < Q.Rows; i++) { -// for (int j = 0; j < Q.Cols; j++) { -// Serial.print(String(Q(i,j)) + "\t"); -// } -// Serial.println(""); -// } - -// Serial.println("<----- Initial Error Cov ----->"); -// for (int i = 0; i < P.Rows; i++) { -// for (int j = 0; j < P.Cols; j++) { -// Serial.print(String(P(i,j)) + "\t"); -// } -// Serial.println(""); -// } -// #endif - -// } -// void StateEstimator::init(BLA::Matrix<3, 1> LLA){ -// // Accelerometer -// BLA::Matrix<3,1> a_b = { -// IMUData->accelX, -// IMUData->accelY, -// IMUData->accelZ -// }; -// a_b = a_b / BLA::Norm(a_b); - -// // Flip gravity direction: ensure z points DOWN in body frame -// float ax = a_b(0), ay = a_b(1), az = a_b(2); - -// // Compute roll (phi) and pitch (theta) assuming NED frame -// float roll = atan2(-ay, -az); // Flip signs to match z-down NED -// float pitch = atan2(ax, sqrt(ay*ay + az*az)); - -// // Magnetometer -// BLA::Matrix<3,1> m_b = { -// IMUData->magX, -// IMUData->magY, -// IMUData->magZ -// }; - -// // Tilt compensation -// float mx = m_b(0), my = m_b(1), mz = m_b(2); - -// float mx2 = mx * cos(pitch) + mz * sin(pitch); -// float my2 = mx * sin(roll) * sin(pitch) + my * cos(roll) - mz * sin(roll) * cos(pitch); - -// float yaw = atan2(-my2, mx2); // Heading angle (NED convention) - -// // Convert roll/pitch/yaw to quaternion (ZYX convention) -// float cy = cos(yaw * 0.5f); -// float sy = sin(yaw * 0.5f); -// float cp = cos(pitch * 0.5f); -// float sp = sin(pitch * 0.5f); -// float cr = cos(roll * 0.5f); -// float sr = sin(roll * 0.5f); - -// BLA::Matrix<4,1> q0 = { -// cr * cp * cy + sr * sp * sy, // w -// sr * cp * cy - cr * sp * sy, // x -// cr * sp * cy + sr * cp * sy, // y -// cr * cp * sy - sr * sp * cy // z -// }; +#include "qmekf.h" + +#include "BasicLinearAlgebra.h" + +#include + +#include "QuaternionUtils.h" + +void StateEstimator::init(BLA::Matrix<3, 1> ECEF, float curr_time) { + if (ECEF(0) != 0) { + launch_ecef = ECEF; + } + + x = {0, 0, 0, 0, + 0, 0, 0, + launch_ecef(0), launch_ecef(1), launch_ecef(2), + vimu_const::gyro_bias(0), vimu_const::gyro_bias(0), vimu_const::gyro_bias(0), + vimu_const::accel_bias(0), vimu_const::accel_bias(1), vimu_const::accel_bias(2), + icm20948_const::mag_bias(0), icm20948_const::mag_bias(1), icm20948_const::mag_bias(2), + lps22_const::baro_bias(0)}; + + + // TODO idk figure this out + P.Fill(0.0f); + for(uint8_t idx : QMEKFInds::quat) { + P(idx, idx) = 1e-8; + } + for (uint8_t idx : QMEKFInds::vel) { + P(idx, idx) = 1e-8; + } + for (uint8_t idx : QMEKFInds::pos) { + P(idx, idx) = 1e-8; + } + for(uint8_t idx : QMEKFInds::gyroBias) { + P(idx, idx) = powf(icm20948_const::gyro_VRW, 2.0f); + } + for(uint8_t idx: QMEKFInds::accelBias) { + P(idx, idx) = powf(icm20948_const::accelXY_VRW, 2.0f); + } + for(uint8_t idx : QMEKFInds::magBias) { + P(idx, idx) = powf(0.1f, 2); + } + for (uint8_t idx : QMEKFInds::baroBias) { + P(idx, idx) = powf(0.1f, 2); + } + + launch_dcmned2ecef = QuaternionUtils::dcm_ned2ecef(QuaternionUtils::ecef2lla(launch_ecef)); + + numLoop = 0; + sumAccel = {0, 0, 0}; + sumMag = {0, 0, 0}; + + lastTimes = {curr_time, curr_time, curr_time, curr_time, curr_time, curr_time}; +} + +void StateEstimator::padLoop(BLA::Matrix<3, 1> accel, BLA::Matrix<3, 1> mag, BLA::Matrix<3, 1> gps_pos) { + sumAccel(0) = sumAccel(0) + accel(0); + sumAccel(1) = sumAccel(1) + accel(1); + sumAccel(2) = sumAccel(2) + accel(2); + sumMag(0) = sumMag(0) + mag(0); + sumMag(1) = sumMag(1) + mag(1); + sumMag(2) = sumMag(2) + mag(2); + + if (gps_pos(0) != 0) { + launch_ecef = gps_pos; + } + + numLoop = numLoop + 1; +} + +void StateEstimator::computeInitialOrientation() { + BLA::Matrix<3, 1> normal_i = QuaternionUtils::normal_i_ecef(launch_dcmned2ecef); + BLA::Matrix<3, 1> m_i = QuaternionUtils::m_i_ecef(launch_dcmned2ecef); + BLA::Matrix<3, 1> normal_b = sumAccel / numLoop; + BLA::Matrix<3, 1> m_b = sumMag / numLoop; + BLA::Matrix initial_quat = QuaternionUtils::triad_BE(normal_b, m_b, normal_i, m_i); + + + x(0) = initial_quat(0); + x(1) = initial_quat(1); + x(2) = initial_quat(2); + x(3) = initial_quat(3); +} -// this->x = {q0(0), q0(1), q0(2), q0(3), -// 0, 0, 0, // velocity -// 0, 0, 0, // position -// 0, 0, 0, // gyro bias -// 0, 0, 0, // accel bias -// 0, 0, 0, // mag bias -// 0}; // baro bias +BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float curr_time) { + float dt = curr_time - QuaternionUtils::vecMax(QuaternionUtils::extractSub(lastTimes, {0, 1, 3, 4}));; + BLA::Matrix<3, 1> unbiased_gyro = gyro; -// this->x_min = x; -// // Set launch site LLA/ECEF -// launch_ecef = QuaternionUtils::lla2ecef(LLA); -// launch_lla = LLA; -// BLA::Matrix<2, 1> ll = {launch_lla(0), launch_lla(1)}; -// R_ET = QuaternionUtils::dcm_ned2ecef(ll); + gyro_prev = unbiased_gyro; -// lastTimes = {millis(), millis(), millis(), millis(), millis()}; -// } + return x; +} // BLA::Matrix<20,1> StateEstimator::onLoop(int state) { diff --git a/src/qmekf.h b/src/qmekf.h index 6a299cb..c0d0c76 100644 --- a/src/qmekf.h +++ b/src/qmekf.h @@ -1,190 +1,155 @@ -// #pragma once - -// #include "BasicLinearAlgebra.h" - -// #include "kfConsts.h" -// #include -// #include "QuaternionUtils.h" - -// /** -// * @name QMEKfInds -// * @brief Struct holding the indices of the total QMEKF -// */ -// namespace QMEKFInds { -// constexpr uint8_t q_w = 0; -// constexpr uint8_t q_x = 1; -// constexpr uint8_t q_y = 2; -// constexpr uint8_t q_z = 3; -// constexpr std::array quat = {q_w, q_x, q_y, q_z}; - -// constexpr uint8_t v_x = 4; -// constexpr uint8_t v_y = 5; -// constexpr uint8_t v_z = 6; -// constexpr std::array vel = {v_x, v_y, v_z}; - -// constexpr uint8_t p_x = 7; -// constexpr uint8_t p_y = 8; -// constexpr uint8_t p_z = 9; -// constexpr std::array pos = {p_x, p_y, p_z}; - -// constexpr uint8_t gb_x = 10; -// constexpr uint8_t gb_y = 11; -// constexpr uint8_t gb_z = 12; -// constexpr std::array gyroBias = {gb_x, gb_y, gb_z}; - -// constexpr uint8_t ab_x = 13; -// constexpr uint8_t ab_y = 14; -// constexpr uint8_t ab_z = 15; -// constexpr std::array accelBias = {ab_x, ab_y, ab_z}; - -// constexpr uint8_t mb_x = 16; -// constexpr uint8_t mb_y = 17; -// constexpr uint8_t mb_z = 18; -// constexpr std::array magBias = {mb_x, mb_y, mb_z}; - -// constexpr uint8_t bb_z = 19; -// constexpr std::array baroBias = {bb_z}; - - - -// }; // namespace QMEKFInds - -// #define QMEKF_LOG_DESC(X) \ -// X(0, "w", p.print(state(QMEKFInds::q_w), 4)) \ -// X(1, "i", p.print(state(QMEKFInds::q_x), 4)) \ -// X(2, "j", p.print(state(QMEKFInds::q_y), 4)) \ -// X(3, "k", p.print(state(QMEKFInds::q_z), 4)) \ -// X(4, "v_x", p.print(state(QMEKFInds::v_x), 4)) \ -// X(5, "v_y", p.print(state(QMEKFInds::v_y), 4)) \ -// X(6, "v_z", p.print(state(QMEKFInds::v_z), 4)) \ -// X(7, "p_x", p.print(state(QMEKFInds::p_x), 4)) \ -// X(8, "p_y", p.print(state(QMEKFInds::p_y), 4)) \ -// X(9, "p_z", p.print(state(QMEKFInds::p_z), 4)) \ - - -// /** -// * @name QMEKFStateEstimator -// * @author QMEKF team -// * @brief Attitude and Position/Velocity estimation. See matlab simulation for details -// */ -// class StateEstimator { - -// public: -// StateEstimator(const TimedPointer, const TimedPointer, float dt); - -// /** -// * @name init -// * @author @frostydev99 -// * @param x_0 - Initial State -// * @param dt - Discrete time step -// */ -// void init(BLA::Matrix<3, 1> LLA); - -// /** -// * @name onLoop -// * @author @frostydev99 -// * @brief Run Every Loop -// * @paragraph This method should run every loop of the expected prediction -// * update rate given by dt -// */ -// BLA::Matrix<20, 1> onLoop(int state); - -// private: -// const TimedPointer IMUData; -// const TimedPointer gpsData; - -// // Prediction Functions -// // BLA::Matrix<20, 1> predictionFunction(BLA::Matrix<20, 1> x, -// // BLA::Matrix<3, 1> gyro, -// // BLA::Matrix<3, 1> accel); -// // BLA::Matrix<19, 19> predictionJacobian(BLA::Matrix<20, 1> x, -// // BLA::Matrix<3, 1> gyro, -// // BLA::Matrix<3, 1> accel); - -// BLA::Matrix<20,1> fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt); -// BLA::Matrix<19, 19> predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt); - -// // Update Functions -// BLA::Matrix<20,1> runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> a_b); - -// BLA::Matrix<20,1> runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> m_b); - -// BLA::Matrix<20,1> runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps); - -// // void runBaroUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<1, 1> baro); - -// BLA::Matrix<20,1> EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R); - -// // State Vector Allocation -// BLA::Matrix<20, 1> x; - -// // Error Covariance Allocation -// BLA::Matrix<19, 19> P; - -// // Identity Matrices -// BLA::Matrix<20, 20> I_20 = BLA::Eye<20, 20>(); -// BLA::Matrix<19, 19> I_19 = BLA::Eye<19, 19>(); -// BLA::Matrix<3, 3> I_3 = BLA::Eye<3, 3>(); - -// //R values -// float accel_var = pow(sqrt(asm330_const::accelXY_var) * 9.8, 2); -// float mag_var = icm20948_const::magXYZ_var; -// float gps_var = Max10S_const::gpsXYZ_var; -// //R matricies -// BLA::Matrix<3, 3> R_accel = {accel_var, 0, 0, -// 0, accel_var, 0, -// 0, 0, accel_var}; - -// BLA::Matrix<3, 3> R_mag = {mag_var, 0, 0, -// 0, mag_var, 0, -// 0, 0, mag_var}; - -// BLA::Matrix<3, 3> R_gps= {gps_var, 0, 0, -// 0, gps_var, 0, -// 0, 0, gps_var}; - -// BLA::Matrix<10, 1> R_all = { -// pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), -// pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), -// pow(sqrt(asm330_const::accelZ_var) * 9.8, 2), -// icm20948_const::magXYZ_var, -// icm20948_const::magXYZ_var, -// icm20948_const::magXYZ_var, -// Max10S_const::gpsXYZ_var, -// Max10S_const::gpsXYZ_var, -// Max10S_const::gpsXYZ_var, -// Max10S_const::baro_var -// }; - - -// BLA::Matrix<5, 1> lastTimes = {0, 0, 0, 0, 0, 0, 0}; -// // Gyro prop, accel prop, accel update, mag update, gps update, baro update - -// BLA::Matrix<3,1> gyro_prev = {0, 0, 0}; -// BLA::Matrix<3,1> accel_prev = {0, 0, 0}; -// BLA::Matrix<3,1> mag_prev = {0, 0, 0}; -// BLA::Matrix<3,1> gps_pos_prev = {0, 0, 0}; -// BLA::Matrix<3,1> gps_vel_prev = {0, 0, 0}; -// BLA::Matrix<1,1> baro_prev = {0}; - -// BLA::Matrix<3, 1> normal_i = {0, 0, -9.8037}; // [m/s^2] -// BLA::Matrix<3, 1> g_i = {0, 0, 9.8037}; // [m/s^2] -// BLA::Matrix<3, 1> m_i = {18.659605, -4.540227, 49.09786}; // [uT] Kids rocket params -// BLA::Matrix<3, 1> launch_ecef = {1311800, -4337300, 4473600}; // [m] // asuming 0 above surface -// BLA::Matrix<3, 1> launch_lla = {44.825070, -73.171726, 0}; // [whatever tf units are in (deg, deg, m)] -// BLA::Matrix<2, 1> launch_ll = {launch_lla(0), launch_lla(1)}; -// BLA::Matrix<3, 3> R_ET = QuaternionUtils::dcm_ned2ecef(launch_ll); - -// template -// BLA::Matrix extractSub(const BLA::Matrix &x, -// const std::array &inds) { -// BLA::Matrix sub; -// for (int i = 0; i < M; i++) { -// sub(i) = x(inds[i]); -// } -// return sub; -// } - -// BLA::Matrix<3, 3> quat2rot(const BLA::Matrix<4, 1> &q); - -// }; +#pragma once + +#include "BasicLinearAlgebra.h" + +#include "kfConsts.h" +#include +#include "QuaternionUtils.h" + +/** + * @name QMEKfInds + * @brief Struct holding the indices of the total QMEKF + */ +namespace QMEKFInds { +constexpr uint8_t q_w = 0; +constexpr uint8_t q_x = 1; +constexpr uint8_t q_y = 2; +constexpr uint8_t q_z = 3; +constexpr std::array quat = {q_w, q_x, q_y, q_z}; + +constexpr uint8_t v_x = 4; +constexpr uint8_t v_y = 5; +constexpr uint8_t v_z = 6; +constexpr std::array vel = {v_x, v_y, v_z}; + +constexpr uint8_t p_x = 7; +constexpr uint8_t p_y = 8; +constexpr uint8_t p_z = 9; +constexpr std::array pos = {p_x, p_y, p_z}; + +constexpr uint8_t gb_x = 10; +constexpr uint8_t gb_y = 11; +constexpr uint8_t gb_z = 12; +constexpr std::array gyroBias = {gb_x, gb_y, gb_z}; + +constexpr uint8_t ab_x = 13; +constexpr uint8_t ab_y = 14; +constexpr uint8_t ab_z = 15; +constexpr std::array accelBias = {ab_x, ab_y, ab_z}; + +constexpr uint8_t mb_x = 16; +constexpr uint8_t mb_y = 17; +constexpr uint8_t mb_z = 18; +constexpr std::array magBias = {mb_x, mb_y, mb_z}; + +constexpr uint8_t bb_z = 19; +constexpr std::array baroBias = {bb_z}; + + + +}; // namespace QMEKFInds + +#define QMEKF_LOG_DESC(X) \ + X(0, "w", p.print(state(QMEKFInds::q_w), 4)) \ + X(1, "i", p.print(state(QMEKFInds::q_x), 4)) \ + X(2, "j", p.print(state(QMEKFInds::q_y), 4)) \ + X(3, "k", p.print(state(QMEKFInds::q_z), 4)) \ + X(4, "v_x", p.print(state(QMEKFInds::v_x), 4)) \ + X(5, "v_y", p.print(state(QMEKFInds::v_y), 4)) \ + X(6, "v_z", p.print(state(QMEKFInds::v_z), 4)) \ + X(7, "p_x", p.print(state(QMEKFInds::p_x), 4)) \ + X(8, "p_y", p.print(state(QMEKFInds::p_y), 4)) \ + X(9, "p_z", p.print(state(QMEKFInds::p_z), 4)) \ + + +/** + * @name QMEKFStateEstimator + * @author QMEKF team + * @brief Attitude and Position/Velocity estimation. See matlab simulation for details + */ +class StateEstimator { + + public: + // State Vector Allocation + // Quat, vel, pos, gyroBias, accelBias, magBias, baroBias. TODO eventually change with real ECEF + BLA::Matrix<20, 1> x; + + // Error Covariance Allocation TODO eventually + BLA::Matrix<19, 19> P; + + /** + * @name init + * @author @frostydev99 + * @param x_0 - Initial State + * @param dt - Discrete time step + */ + void init(BLA::Matrix<3, 1> ECEF, float curr_time); + void padLoop(BLA::Matrix<3, 1> accel, BLA::Matrix<3, 1> mag, BLA::Matrix<3, 1> gps_pos); + void computeInitialOrientation(); + + BLA::Matrix<20, 1> fastGyroProp(BLA::Matrix<3,1> gyro, float curr_time); + BLA::Matrix<20, 1> fastAccelProp(BLA::Matrix<3,1> accel, float curr_time); + BLA::Matrix<19, 19> ekfPredict(float curr_time); + + // Update Functions + BLA::Matrix<20, 1> runAccelUpdate(BLA::Matrix<3, 1> a_b, float curr_time); + BLA::Matrix<20, 1> runMagUpdate(BLA::Matrix<3, 1> m_b, float curr_time); + BLA::Matrix<20, 1> runGPSUpdate(BLA::Matrix<3, 1> gpsPos, BLA::Matrix<3, 1> gpsVel); + BLA::Matrix<20, 1> runBaroUpdate(BLA::Matrix<1, 1> baro); + + private: + // Identity Matrices + BLA::Matrix<20, 20> I_20 = BLA::Eye<20, 20>(); + BLA::Matrix<19, 19> I_19 = BLA::Eye<19, 19>(); + BLA::Matrix<3, 3> I_3 = BLA::Eye<3, 3>(); + + BLA::Matrix<3, 1> launch_ecef = {1475354.0f, -4490428.0f, 4268181.0f}; + BLA::Matrix<3, 3> launch_dcmned2ecef; + + // TODO this one is more confusing because can have diff sizes matrices + BLA::Matrix<20, 1> EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R); + + BLA::Matrix<6, 1> lastTimes = {0, 0, 0, 0, 0, 0}; + // Gyro prop, accel prop, accel update, mag update, gps update, baro update + BLA::Matrix<3,1> gyro_prev = {0, 0, 0}; + BLA::Matrix<3,1> accel_prev = {0, 0, 0}; + BLA::Matrix<3,1> mag_prev = {0, 0, 0}; + BLA::Matrix<3,1> gps_pos_prev = {0, 0, 0}; + BLA::Matrix<3,1> gps_vel_prev = {0, 0, 0}; + BLA::Matrix<1,1> baro_prev = {0}; + + + // padLoop vars: + float numLoop; + BLA::Matrix<3, 1> sumAccel; + BLA::Matrix<3, 1> sumMag; + + // //R matricies + // BLA::Matrix<3, 3> R_accel = {accel_var, 0, 0, + // 0, accel_var, 0, + // 0, 0, accel_var}; + + // BLA::Matrix<3, 3> R_mag = {mag_var, 0, 0, + // 0, mag_var, 0, + // 0, 0, mag_var}; + + // BLA::Matrix<3, 3> R_gps= {gps_var, 0, 0, + // 0, gps_var, 0, + // 0, 0, gps_var}; + + // BLA::Matrix<10, 1> R_all = { + // pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), + // pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), + // pow(sqrt(asm330_const::accelZ_var) * 9.8, 2), + // icm20948_const::magXYZ_var, + // icm20948_const::magXYZ_var, + // icm20948_const::magXYZ_var, + // Max10S_const::gpsXYZ_var, + // Max10S_const::gpsXYZ_var, + // Max10S_const::gpsXYZ_var, + // Max10S_const::baro_var + // }; + + + +}; From 5b44007a59c0b04ef497d4652b1d99e3b59c398d Mon Sep 17 00:00:00 2001 From: krizzyegg Date: Wed, 3 Dec 2025 17:48:09 -0500 Subject: [PATCH 06/14] killed QMEKF --- QMEKF/Conventions.txt | 19 -- QMEKF/kfConsts.h | 41 ---- QMEKF/qmekf.cpp | 510 ------------------------------------------ QMEKF/qmekf.h | 238 -------------------- 4 files changed, 808 deletions(-) delete mode 100644 QMEKF/Conventions.txt delete mode 100644 QMEKF/kfConsts.h delete mode 100644 QMEKF/qmekf.cpp delete mode 100644 QMEKF/qmekf.h diff --git a/QMEKF/Conventions.txt b/QMEKF/Conventions.txt deleted file mode 100644 index eea600a..0000000 --- a/QMEKF/Conventions.txt +++ /dev/null @@ -1,19 +0,0 @@ -Notes: - - -I change magData variable to imuData because it contains all the IMU data, not just magData -Also please change the accel to return the m/s^2, not g's -I change take in int variable state to onLoop, not the boolean: - 0 = preLaunch - 1 = high thrust - 2 = coast - 3 = falling down - - - -Frequencies variable in format: - priori step - accel upate - mag update - gps update - baro update \ No newline at end of file diff --git a/QMEKF/kfConsts.h b/QMEKF/kfConsts.h deleted file mode 100644 index a9e7c98..0000000 --- a/QMEKF/kfConsts.h +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -#include -constexpr static float g = 9.80665; // [m/s/s] Earth's Grav Accel - -constexpr float square(float a) { return a * a; } - -namespace asm330_const { -constexpr float accelXY_var = square(0.0020f); // [g] -constexpr float accelZ_var = square(0.0014f); // [g] -constexpr float accelXY_VRW = 0.0003f; // [g/sqrt(Hz)] -constexpr float accelZ_VRW = 0.0002f; // [g/sqrt(Hz)] -constexpr float gyroX_var = square(0.0947f); // [deg/s] -constexpr float gyroY_var = square(0.1474f); // [deg/s] -constexpr float gyroZ_var = square(0.2144f); // [deg/s] -constexpr float gyro_VRW = 0.0241f; // [deg/s/sqrt(Hz)] -}; // namespace asm330_const - -namespace lps22_const { -constexpr float baro_var = 0.0f; -}; - -namespace icm20948_const { -constexpr float accelXY_var = square(0.0383f); // [g] -constexpr float accelZ_var = square(0.0626f); // [g] -constexpr float accelXY_VRW = 0.0062f; // [g/sqrt(Hz)] -constexpr float accelZ_VRW = 0.0099f; // [g/sqrt(Hz)] -// constexpr float gyroXYZ_var = square(0.0051); // [rad/s] -constexpr float gyroXYZ_var = square(5e-4); // [rad/s] -constexpr float gyro_VRW = 8.33e-4f; // [rad/s/sqrt(Hz)] -constexpr float magXYZ_var = square(0.7263f); // [uT] -constexpr float quatVar = 0.3; // Idk Guess - - -}; // namespace icm20948_const - - -namespace Max10S_const { - constexpr float gpsXYZ_var = square(2.0); // [m] idk guess - constexpr float baro_var = square(7.5); // [P] idk guess -} diff --git a/QMEKF/qmekf.cpp b/QMEKF/qmekf.cpp deleted file mode 100644 index 4b6c1bd..0000000 --- a/QMEKF/qmekf.cpp +++ /dev/null @@ -1,510 +0,0 @@ -#include "ekf-implementation/QMEKF/qmekf.h" - -#include "BasicLinearAlgebra.h" - -#include - -StateEstimator::StateEstimator(const TimedPointer IMUData, - const TimedPointer gpsData, - float dt) : IMUData(IMUData), gpsData(gpsData) { - P.Fill(0.0f); - for(uint8_t idx : QMEKFInds::quat) { - P(idx, idx) = 1e-8; - } - for (uint8_t idx : QMEKFInds::vel) { - P(idx, idx) = 1e-8; - } - for (uint8_t idx : QMEKFInds::pos) { - P(idx, idx) = 1e-8; - } - for(uint8_t idx : QMEKFInds::gyroBias) { - P(idx, idx) = powf(icm20948_const::gyro_VRW, 2.0f); - } - for(uint8_t idx: QMEKFInds::accelBias) { - P(idx, idx) = powf(icm20948_const::accelXY_VRW, 2.0f); - } - for(uint8_t idx : QMEKFInds::magBias) { - // P(idx, idx) = icm20948_const.magXYZ_var; - P(idx, idx) = powf(0.1f, 2); - } - for (uint8_t idx : QMEKFInds::baroBias) { - P(idx, idx) = powf(0.1f, 2); - } - - P_min = P; - - - //ToDo: Look at this alter, big ass matrix or integration - Q.Fill(0.0f); - for(uint8_t idx : QMEKFInds::quat) { - Q(idx, idx) = 1e-8; - } - for(uint8_t idx : QMEKFInds::vel) { - Q(idx, idx) = 1e-8; - } - for(uint8_t idx : QMEKFInds::pos) { - Q(idx, idx) = 1e-8; - } - for(uint8_t idx : QMEKFInds::gyroBias) { - Q(idx, idx) = powf(0.1f,2); - } - for(uint8_t idx : QMEKFInds::accelBias) { - Q(idx, idx) = powf(0.01f,2); - } - for(uint8_t idx : QMEKFInds::magBias) { - Q(idx, idx) = powf(0.1f, 2); - } - for(uint8_t idx : QMEKFInds::baroBias) { - Q(idx, idx) = powf(0.1f, 2); - } - -#ifdef DBG - Serial.println("<----- Process Noise ----->"); - for (int i = 0; i < Q.Rows; i++) { - for (int j = 0; j < Q.Cols; j++) { - Serial.print(String(Q(i,j)) + "\t"); - } - Serial.println(""); - } - - Serial.println("<----- Initial Error Cov ----->"); - for (int i = 0; i < P.Rows; i++) { - for (int j = 0; j < P.Cols; j++) { - Serial.print(String(P(i,j)) + "\t"); - } - Serial.println(""); - } -#endif - -} -void StateEstimator::init(BLA::Matrix<3, 1> LLA){ - // Accelerometer - BLA::Matrix<3,1> a_b = { - IMUData->accelX, - IMUData->accelY, - IMUData->accelZ - }; - a_b = a_b / BLA::Norm(a_b); - - // Flip gravity direction: ensure z points DOWN in body frame - float ax = a_b(0), ay = a_b(1), az = a_b(2); - - // Compute roll (phi) and pitch (theta) assuming NED frame - float roll = atan2(-ay, -az); // Flip signs to match z-down NED - float pitch = atan2(ax, sqrt(ay*ay + az*az)); - - // Magnetometer - BLA::Matrix<3,1> m_b = { - IMUData->magX, - IMUData->magY, - IMUData->magZ - }; - - // Tilt compensation - float mx = m_b(0), my = m_b(1), mz = m_b(2); - - float mx2 = mx * cos(pitch) + mz * sin(pitch); - float my2 = mx * sin(roll) * sin(pitch) + my * cos(roll) - mz * sin(roll) * cos(pitch); - - float yaw = atan2(-my2, mx2); // Heading angle (NED convention) - - // Convert roll/pitch/yaw to quaternion (ZYX convention) - float cy = cos(yaw * 0.5f); - float sy = sin(yaw * 0.5f); - float cp = cos(pitch * 0.5f); - float sp = sin(pitch * 0.5f); - float cr = cos(roll * 0.5f); - float sr = sin(roll * 0.5f); - - BLA::Matrix<4,1> q0 = { - cr * cp * cy + sr * sp * sy, // w - sr * cp * cy - cr * sp * sy, // x - cr * sp * cy + sr * cp * sy, // y - cr * cp * sy - sr * sp * cy // z - }; - - this->x = {q0(0), q0(1), q0(2), q0(3), - 0, 0, 0, // velocity - 0, 0, 0, // position - 0, 0, 0, // gyro bias - 0, 0, 0, // accel bias - 0, 0, 0, // mag bias - 0}; // baro bias - - this->x_min = x; - - // Set launch site LLA/ECEF - launch_ecef = QuaternionUtils::lla2ecef(LLA); - launch_lla = LLA; - BLA::Matrix<2, 1> ll = {launch_lla(0), launch_lla(1)}; - R_ET = QuaternionUtils::dcm_ned2ecef(ll); - - lastTimes = {millis(), millis(), millis(), millis(), millis()}; -} - - -BLA::Matrix<20,1> StateEstimator::onLoop(int state) { - // Read data from sensors and convert values - - float dt; - - float gyrX = IMUData->gyrX; - float gyrY = IMUData->gyrY; - float gyrZ = IMUData->gyrZ; - - float aclX = IMUData->accelX; - float aclY = IMUData->accelY; - float aclZ = IMUData->accelZ; - - float magX = IMUData->magX; - float magY = IMUData->magY; - float magZ = IMUData->magZ; - - - // TODO get in the GPS data and baro data from somewhere - BLA::Matrix<3,1> gyro = {gyrX, gyrY, gyrZ}; // [rad/s] - BLA::Matrix<3,1> accel = {aclX, aclY, aclZ}; // [m/s^s] - BLA::Matrix<3,1> mag = {magX, magY, magZ}; // [uT] - BLA::Matrix<3,1> gpsData = {gpsData(0), gpsData(1), gpsData(2)}; - // BLA::Matrix<1,1> baro = {baroZ}; - - // Remove biases from each measurement - BLA::Matrix<3,1> unbiased_gyro = {gyro(0) - x(QMEKFInds::gb_x), gyro(1) - x(QMEKFInds::gb_y), gyro(2) - x(QMEKFInds::gb_z)}; - BLA::Matrix<3,1> unbiased_accel = {accel(0) - x(QMEKFInds::ab_x), accel(1) - x(QMEKFInds::ab_y), accel(2) - x(QMEKFInds::ab_z)}; - BLA::Matrix<3,1> unbiased_mag = {mag(0) - x(QMEKFInds::mb_x), mag(1) - x(QMEKFInds::mb_y), mag(2) - x(QMEKFInds::mb_z)}; - // BLA::Matrix<1,1> unbiased_baro = {baro(0) - x(20)}; - - - float time = millis(); - bool run_priori = time - lastTimes(0) >= frequencies(0); - bool run_accel_update = time - lastTimes(1) >= frequencies(1); - bool run_mag_update = time - lastTimes(2) >= frequencies(2); - bool run_gps_update = time - lastTimes(3) >= frequencies(3); - // bool run_baro_update = time - lastTimeBaro(4) >= frequencies(4); - - - if(run_priori) { - // TODO eventually implement RK4 here, but I don't understand it yet - float lastAttUpdate = max(max(lastTimes(0), lastTimes(1)), lastTimes(2)); // Maximum of the lastTimes(0, 1, 2) - float lastPVUpdate = max(max(lastTimes(0), lastTimes(3)), lastTimes(4)); // Maximum of the lastTimes(0, 3, 4) - float dt_att = millis() - lastAttUpdate; - float dt_pv = millis() - lastPVUpdate; - - x = fastIMUProp(gyro, accel, dt_att, dt_pv); - - - lastTimes(0) = millis(); - } - - if (run_accel_update || run_mag_update || run_gps_update) { - dt = millis() - max(max(lastTimes(0), lastTimes(1)), max(lastTimes(2), lastTimes(3)));; - P = predictionFunction(P, gyro, accel, dt); - } - - if (run_accel_update) { - runAccelUpdate(x, accel); - lastTimes(1) = millis(); - } - - if (run_mag_update) { - runMagUpdate(x, mag); - lastTimes(2) = millis(); - } - - if (run_gps_update) { - BLA::Matrix<3,1> gps_ecef = QuaternionUtils::lla2ecef(gpsData); - runGPSUpdate(x, gps_ecef); - lastTimes(3) = millis(); - } - - // if (run_baro_update) { - // run_baro_update(); - // lastTimes(4) = millis(); - // } - - //Update sensor readings - gyro_prev = unbiased_gyro; - accel_prev = unbiased_accel; - mag_prev = unbiased_mag; - gps_prev = gpsData; - // baro_prev = unbiased_baro; - - return x; -} - -BLA::Matrix<20,1> StateEstimator::fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt) { - - BLA::Matrix<3, 1> v; - BLA::Matrix<3, 1> p; - - // TODO change to from world model when go to ECEF - BLA::Matrix<3,1> gyro_int = {gyro(0)*att_dt, gyro(1)*att_dt, gyro(2)*att_dt}; - float rotVecNorm = BLA::Norm(gyro_int); - BLA::Matrix<3,1> axis = {(gyro_int(0) / rotVecNorm), (gyro_int(1) / rotVecNorm), (gyro_int(2) / rotVecNorm)}; - BLA::Matrix<4,1> dq = - { - cos(rotVecNorm/2.0f), - axis(0) * sinf(rotVecNorm/2.0f), - axis(1) * sinf(rotVecNorm/2.0f), - axis(2) * sinf(rotVecNorm/2.0f), - }; - BLA::Matrix<4,1> q = - { - x(QMEKFInds::q_w), - x(QMEKFInds::q_x), - x(QMEKFInds::q_y), - x(QMEKFInds::q_z), - }; - q = QuaternionUtils::quatMultiply(q, dq); - BLA::Matrix<4,1> qNorm = {q(0) / BLA::Norm(q), q(1) / BLA::Norm(q), q(2) / BLA::Norm(q), q(3) / BLA::Norm(q)}; - - - - BLA::Matrix<3,1> v_dot = QuaternionUtils::quatToRot(q) * accel + g_i; - BLA::Matrix<3,1> old_v = { - x(QMEKFInds::v_x), x(QMEKFInds::v_y), x(QMEKFInds::v_z) - }; - BLA::Matrix<3,1> old_p = { - x(QMEKFInds::p_x), x(QMEKFInds::p_y), x(QMEKFInds::p_z) - }; - - v = old_v + v_dot * pv_dt; - p = old_p + v * pv_dt; - - x(QMEKFInds::v_x) = v(0); - x(QMEKFInds::v_y) = v(1); - x(QMEKFInds::v_z) = v(2); - x(QMEKFInds::p_x) = p(0); - x(QMEKFInds::p_y) = p(1); - x(QMEKFInds::p_z) = p(2); - - return x; - -} - -BLA::Matrix<19, 19> StateEstimator::predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt) { - BLA::Matrix<3,3> gyroSkew = QuaternionUtils::skewSymmetric(gyroVec); - BLA::Matrix<3,3> accelSkew = QuaternionUtils::skewSymmetric(accelVec); - - BLA::Matrix<4, 1> q = - { - x(QMEKFInds::q_w), - x(QMEKFInds::q_x), - x(QMEKFInds::q_y), - x(QMEKFInds::q_z) - }; - - BLA::Matrix<3, 3> rotMatrix = QuaternionUtils::quatToRot(q); - - BLA::Matrix<19, 19> F; - F.Fill(0); - - //Row 1 - 3 - F.Submatrix<3, 3>(0, QMEKFInds::q_w) = -1.0f * gyroSkew; - F.Submatrix<3, 3>(0, QMEKFInds::gb_x) = -1.0f * I_3; - - //Row 4 - 6 - F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::q_w) = -1.0f * rotMatrix * accelSkew; - F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::ab_x) = -1.0f * rotMatrix; - - //Row 7 - 9 - F.Submatrix<3, 3>(QMEKFInds::p_x -1, 3) = I_3; - - BLA::Matrix<19, 19> phi; - phi.Fill(0); - - phi = I_19 + (F * dt) + (0.5f * F * F * float(pow(dt, 2))); - - BLA::Matrix<19, 19> phi_t = ~phi; - - BLA::Matrix<19, 19> Q_d; - Q_d.Fill(0); - - BLA::Matrix<3, 3> gyro_var_diag; - gyro_var_diag.Fill(0); - gyro_var_diag(0, 0) = QMEKFInds::gyro_var; - gyro_var_diag(1, 1) = QMEKFInds::gyro_var; - gyro_var_diag(2, 2) = QMEKFInds::gyro_var; - - BLA::Matrix<3, 3> gyro_bias_var_diag; - gyro_bias_var_diag.Fill(0); - gyro_bias_var_diag(0, 0) = QMEKFInds::gyro_bias_var; - gyro_bias_var_diag(1, 1) = QMEKFInds::gyro_bias_var; - gyro_bias_var_diag(2, 2) = QMEKFInds::gyro_bias_var; - - BLA::Matrix<3, 3> accel_bias_var_diag; - accel_bias_var_diag.Fill(0); - accel_bias_var_diag(0, 0) = QMEKFInds::accel_bias_var; - accel_bias_var_diag(1, 1) = QMEKFInds::accel_bias_var; - accel_bias_var_diag(2, 2) = QMEKFInds::accel_bias_var; - - Q_d.Submatrix<3, 3>(QMEKFInds::q_w, QMEKFInds::q_w) = (gyro_var_diag * dt) + (gyro_bias_var_diag * float((pow(dt, 3) / 10))); - Q_d.Submatrix<3, 3>(QMEKFInds::q_w, 9) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2)); - - BLA::Matrix<3, 3> R_grav_diag; - R_grav_diag.Fill(0); - R_grav_diag(0, 0) = QMEKFInds::R_grav; - R_grav_diag(1, 1) = QMEKFInds::R_grav; - R_grav_diag(2, 2) = QMEKFInds::R_grav; - - Q_d.Submatrix<3 ,3>(3, 3) = R_grav_diag * dt + accel_bias_var_diag * float((pow(dt, 3) / 3)); - Q_d.Submatrix<3, 3>(3, 6) = accel_bias_var_diag * float((pow(dt ,4) / 8.0)) + R_grav_diag * float((pow(dt, 2) / 2.0)); - Q_d.Submatrix<3, 3>(3, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); - - Q_d.Submatrix<3, 3>(6, 3) = R_grav_diag * float((pow(dt, 2) / 2)) + accel_bias_var_diag * float((pow(dt, 4) / 8.0)); - Q_d.Submatrix<3, 3>(6, 6) = R_grav_diag * float((pow(dt, 3) / 3.0)) + accel_bias_var_diag * float((pow(dt, 5) / 20.0)); - Q_d.Submatrix<3, 3>(6, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 3) / 6.0)); - - Q_d.Submatrix<3, 3>(9, 0) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); - Q_d.Submatrix<3, 3>(9, 9) = gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); - - Q_d.Submatrix<3, 3>(12, 3) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); - Q_d.Submatrix<3, 3>(12, 6) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); - Q_d.Submatrix<3, 3>(12, 12) = accel_bias_var_diag * dt; - - Q_d(15, 15) = QMEKFInds::mag_bias_var * dt; - - Q_d(18, 18) = QMEKFInds::baro_bias_var * dt; - - BLA::Matrix<19, 19> P; - - P = phi * P_ * phi_t + Q_d; - - return P; - -} - -BLA::Matrix<20, 1> StateEstimator::runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3,1> accel_meas) -{ - BLA::Matrix<4,1> q = - { - x(QMEKFInds::q_w), - x(QMEKFInds::q_x), - x(QMEKFInds::q_y), - x(QMEKFInds::q_z), - }; - - BLA::Matrix<3, 19> H_accel; - H_accel.Fill(0); - H_accel.Submatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * (-1.0f * normal_i)); - H_accel.Submatrix<3, 3>(0, QMEKFInds::ab_x - 1) = -1.0f * QuaternionUtils::quat2DCM(q); - - BLA::Matrix<3, 1> h_accel; - - h_accel = QuaternionUtils::quat2DCM(q) * normal_i; - - EKFCalcErrorInject(x, P, accel_meas, H_accel, h_accel, R_accel); - -} - -BLA::Matrix<20, 1> StateEstimator::runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> mag_meas) { - // TODO input igrm model somehow figure out - BLA::Matrix<4, 1> q = - { - x(QMEKFInds::q_w), - x(QMEKFInds::q_x), - x(QMEKFInds::q_y), - x(QMEKFInds::q_z) - }; - - BLA::Matrix<3, 19> H_mag; - H_mag.Fill(0); - H_mag.Submatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * m_i); - H_mag.Submatrix<3, 3>(0, QMEKFInds::gb_x) = I_3; - - BLA::Matrix<3, 1> h_mag; - - h_mag = QuaternionUtils::quat2DCM(q) * m_i; - - EKFCalcErrorInject(x, P, mag_meas, H_mag, h_mag, R_mag); - -} - -BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps_meas_ecef) { - BLA::Matrix<3, 1> pos_ned = QuaternionUtils::ecef2ned(gps_meas_ecef, launch_ecef, R_ET); - - BLA::Matrix<3, 19> H_gps; - H_gps.Fill(0); - H_gps.Submatrix<3, 3>(0, QMEKFInds::p_x) = I_3; - - BLA::Matrix<3, 1> h_gps = { - x(QMEKFInds::p_x), - x(QMEKFInds::p_y), - x(QMEKFInds::p_z), - }; - - EKFCalcErrorInject(x, P, pos_ned, H_gps, h_gps, R_gps); - -} - -BLA::Matrix<20, 1> StateEstimator::EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R) { - BLA::Matrix<3, 1> residual; - residual = sens_reading - h; - - BLA::Matrix<3, 3> S; - BLA::Matrix<19, 3> K; - BLA::Matrix<19, 3> H_t = ~H; - - S = H * oldP * H_t + R; - K = (oldP * H_t) * BLA::Inverse(S); - BLA::Matrix<19, 1> postErrorState = K * residual; - - // Inject error angles into quat - BLA::Matrix<3, 1> alpha; - alpha = {postErrorState(0), postErrorState(1), postErrorState(2)}; - BLA::Matrix<3, 1> rotVec = 1.0f * alpha; - float rotVecNorm = BLA::Norm(rotVec); - BLA::Matrix<3,1> axis = rotVec / rotVecNorm; - BLA::Matrix<4,1> dq = - { - cos(rotVecNorm/2.0f), - axis(0) * sinf(rotVecNorm/2.0f), - axis(1) * sinf(rotVecNorm/2.0f), - axis(2) * sinf(rotVecNorm/2.0f), - }; - BLA::Matrix<4, 1> old_q = - { - x(0), - x(1), - x(2), - x(3) - }; - BLA::Matrix<4, 1> q = QuaternionUtils::quatMultiply(old_q, dq); - - // Set quats - x(0) = q(0); - x(1) = q(1); - x(2) = q(2); - x(3) = q(3); - - // Set velocity - x(4) = x(4) + postErrorState(3); - x(5) = x(5) + postErrorState(4); - x(6) = x(6) + postErrorState(5); - - // Set position - x(7) = x(7) + postErrorState(6); - x(8) = x(8) + postErrorState(7); - x(9) = x(9) + postErrorState(8); - - // Set gyro bias - x(10) = x(10) + postErrorState(9); - x(11) = x(11) + postErrorState(10); - x(12) = x(12) + postErrorState(11); - - // Set accel bias - x(13) = x(13) + postErrorState(12); - x(14) = x(14) + postErrorState(13); - x(15) = x(15) + postErrorState(14); - - // Set mag bias - x(16) = x(16) + postErrorState(15); - x(17) = x(17) + postErrorState(16); - x(18) = x(18) + postErrorState(17); - - // Set baro bias - x(19) = x(19) + postErrorState(18); - - return x; -} - diff --git a/QMEKF/qmekf.h b/QMEKF/qmekf.h deleted file mode 100644 index 79a5be0..0000000 --- a/QMEKF/qmekf.h +++ /dev/null @@ -1,238 +0,0 @@ -#pragma once - -#include "BasicLinearAlgebra.h" - -#include "kfConsts.h" -#include - -/** - * @name QMEKfInds - * @brief Struct holding the indices of the total QMEKF - */ -namespace QMEKFInds { -constexpr uint8_t q_w = 0; -constexpr uint8_t q_x = 1; -constexpr uint8_t q_y = 2; -constexpr uint8_t q_z = 3; -constexpr std::array quat = {q_w, q_x, q_y, q_z}; - -constexpr uint8_t v_x = 4; -constexpr uint8_t v_y = 5; -constexpr uint8_t v_z = 6; -constexpr std::array vel = {v_x, v_y, v_z}; - -constexpr uint8_t p_x = 7; -constexpr uint8_t p_y = 8; -constexpr uint8_t p_z = 9; -constexpr std::array pos = {p_x, p_y, p_z}; - -constexpr uint8_t gb_x = 10; -constexpr uint8_t gb_y = 11; -constexpr uint8_t gb_z = 12; -constexpr std::array gyroBias = {gb_x, gb_y, gb_z}; - -constexpr uint8_t ab_x = 13; -constexpr uint8_t ab_y = 14; -constexpr uint8_t ab_z = 15; -constexpr std::array accelBias = {ab_x, ab_y, ab_z}; - -constexpr uint8_t mb_x = 16; -constexpr uint8_t mb_y = 17; -constexpr uint8_t mb_z = 18; -constexpr std::array magBias = {mb_x, mb_y, mb_z}; - -constexpr uint8_t bb_z = 19; -constexpr std::array baroBias = {bb_z}; - -constexpr float gyro_var = 0.0051; -constexpr float gyro_bias_var = pow(pow(4.9, -5) * 9.8, 2); - -constexpr float accel_var = pow(4.9 * pow(10, -5) * 9.9, 2); -constexpr float accel_bias_var = pow(0.00098 * 9.8, 2); - -constexpr float mag_bias_var = 25; -constexpr float baro_bias_var = pow(7.5, 2); - -constexpr float R_grav = pow(sqrt(asm330_const::accelXY_var) * 9.8, 2); - -}; // namespace QMEKFInds - -#define QMEKF_LOG_DESC(X) \ - X(0, "w", p.print(state(QMEKFInds::q_w), 4)) \ - X(1, "i", p.print(state(QMEKFInds::q_x), 4)) \ - X(2, "j", p.print(state(QMEKFInds::q_y), 4)) \ - X(3, "k", p.print(state(QMEKFInds::q_z), 4)) \ - X(4, "v_x", p.print(state(QMEKFInds::v_x), 4)) \ - X(5, "v_y", p.print(state(QMEKFInds::v_y), 4)) \ - X(6, "v_z", p.print(state(QMEKFInds::v_z), 4)) \ - X(7, "p_x", p.print(state(QMEKFInds::p_x), 4)) \ - X(8, "p_y", p.print(state(QMEKFInds::p_y), 4)) \ - X(9, "p_z", p.print(state(QMEKFInds::p_z), 4)) \ - - -class QMEKFLogger : public Loggable { - public: - QMEKFLogger() : Loggable(NUM_FIELDS(QMEKF_LOG_DESC)) {} - - void newState(BLA::Matrix<20, 1> s) { - initialized = true; - state = s; - updatedAt = millis(); - } - - const BLA::Matrix<20, 1>& getState() { return state; } - - private: - BLA::Matrix<20, 1> state; - uint32_t updatedAt = 0; - bool initialized = false; - - MAKE_LOGGABLE(QMEKF_LOG_DESC) - - uint32_t dataUpdatedAt() override { - if (!initialized) return 0; // Data will not be loggged - return updatedAt; - } -}; - - - - -/** - * @name QMEKFStateEstimator - * @author QMEKF team - * @brief Attitude and Position/Velocity estimation. See matlab simulation for details - */ -class StateEstimator { - - public: - StateEstimator(const TimedPointer, const TimedPointer, float dt); - - /** - * @name init - * @author @frostydev99 - * @param x_0 - Initial State - * @param dt - Discrete time step - */ - void init(BLA::Matrix<3, 1> LLA); - - /** - * @name onLoop - * @author @frostydev99 - * @brief Run Every Loop - * @paragraph This method should run every loop of the expected prediction - * update rate given by dt - */ - BLA::Matrix<20, 1> onLoop(int state); - - private: - const TimedPointer IMUData; - const TimedPointer gpsData; - - // Prediction Functions - // BLA::Matrix<20, 1> predictionFunction(BLA::Matrix<20, 1> x, - // BLA::Matrix<3, 1> gyro, - // BLA::Matrix<3, 1> accel); - // BLA::Matrix<19, 19> predictionJacobian(BLA::Matrix<20, 1> x, - // BLA::Matrix<3, 1> gyro, - // BLA::Matrix<3, 1> accel); - - BLA::Matrix<20,1> fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt); - BLA::Matrix<19, 19> predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt); - - // Update Functions - BLA::Matrix<20,1> runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> a_b); - - BLA::Matrix<20,1> runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> m_b); - - BLA::Matrix<20,1> runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps); - - // void runBaroUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<1, 1> baro); - - BLA::Matrix<20,1> EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R); - - // State Vector Allocation - BLA::Matrix<20, 1> x_min; - - BLA::Matrix<20, 1> x; - - // Error Covariance Allocation - BLA::Matrix<19, 19> P; - - BLA::Matrix<19, 19> P_min; - - // Process Noise Covariance Allocation - BLA::Matrix<19, 19> Q; - - // Identity Matrices - BLA::Matrix<20, 20> I_20 = BLA::Eye<20, 20>(); - BLA::Matrix<19, 19> I_19 = BLA::Eye<19, 19>(); - BLA::Matrix<3, 3> I_3 = BLA::Eye<3, 3>(); - - //R values - float accel_var = pow(sqrt(asm330_const::accelXY_var) * 9.8, 2); - float mag_var = icm20948_const::magXYZ_var; - float gps_var = Max10S_const::gpsXYZ_var; - //R matricies - BLA::Matrix<3, 3> R_accel = {accel_var, 0, 0, - 0, accel_var, 0, - 0, 0, accel_var}; - - BLA::Matrix<3, 3> R_mag = {mag_var, 0, 0, - 0, mag_var, 0, - 0, 0, mag_var}; - - BLA::Matrix<3, 3> R_gps= {gps_var, 0, 0, - 0, gps_var, 0, - 0, 0, gps_var}; - - BLA::Matrix<10, 1> R_all = { - pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), - pow(sqrt(asm330_const::accelXY_var) * 9.8, 2), - pow(sqrt(asm330_const::accelZ_var) * 9.8, 2), - icm20948_const::magXYZ_var, - icm20948_const::magXYZ_var, - icm20948_const::magXYZ_var, - Max10S_const::gpsXYZ_var, - Max10S_const::gpsXYZ_var, - Max10S_const::gpsXYZ_var, - Max10S_const::baro_var - }; - - - BLA::Matrix<5, 1> lastTimes; - - // In the format defined above, how often to run stuff - BLA::Matrix<5, 1> frequencies = { - 100, - 300, - 400, - 2000, - 1000}; - - BLA::Matrix<3,1> gyro_prev; - BLA::Matrix<3,1> accel_prev; - BLA::Matrix<3,1> mag_prev; - BLA::Matrix<3,1> gps_prev; - - BLA::Matrix<3, 1> normal_i = {0, 0, -9.8037}; // [m/s^2] - BLA::Matrix<3, 1> g_i = {0, 0, 9.8037}; // [m/s^2] - BLA::Matrix<3, 1> m_i = {18.659605, -4.540227, 49.09786}; // [uT] Kids rocket params - BLA::Matrix<3, 1> launch_ecef = {1311800, -4337300, 4473600}; // [m] // asuming 0 above surface - BLA::Matrix<3, 1> launch_lla = {44.825070, -73.171726, 0}; // [whatever tf units are in (deg, deg, m)] - BLA::Matrix<2, 1> launch_ll = {launch_lla(0), launch_lla(1)}; - BLA::Matrix<3, 3> R_ET = QuaternionUtils::dcm_ned2ecef(launch_ll); - -template -BLA::Matrix extractSub(const BLA::Matrix &x, - const std::array &inds) { - BLA::Matrix sub; - for (int i = 0; i < M; i++) { - sub(i) = x(inds[i]); - } - return sub; -} - -BLA::Matrix<3, 3> quat2rot(const BLA::Matrix<4, 1> &q); - -}; From 83cc373b071797bea2b07706744c68befe910587 Mon Sep 17 00:00:00 2001 From: Abhay Mathur <66543532+abhayma1000@users.noreply.github.com> Date: Wed, 3 Dec 2025 17:49:56 -0500 Subject: [PATCH 07/14] Got rid of errors --- src/QuaternionUtils.h | 56 +++++++++++++++++++++---------------------- src/qmekf.cpp | 2 +- 2 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/QuaternionUtils.h b/src/QuaternionUtils.h index eb8892a..64cc06f 100644 --- a/src/QuaternionUtils.h +++ b/src/QuaternionUtils.h @@ -59,32 +59,32 @@ namespace QuaternionUtils { BLA::Matrix<3, 1> ecef2lla(BLA::Matrix<3, 1> ecef); - template - BLA::Matrix extractSub(const BLA::Matrix &x, - const std::array &inds) { - BLA::Matrix sub; - for (int i = 0; i < M; i++) { - sub(i) = x(inds[i]); - } - return sub; - } - - template - BLA::Matrix extractDiag(const BLA::Matrix &x) { - BLA::Matrix diag; - for (int i = 0; i < M; i++) { - diag(i) = x(inds[i], indx[i]); - } - return diag; - } - - template - float vecMax(const BLA::Matrix &x) { - BLA::Matrix diag; - for (int i = 0; i < M; i++) { - diag(i) = x(inds[i], indx[i]); - } - return diag; - // TODO fix - } + // template + // BLA::Matrix extractSub(const BLA::Matrix &x, + // const std::array &inds) { + // BLA::Matrix sub; + // for (int i = 0; i < M; i++) { + // sub(i) = x(inds[i]); + // } + // return sub; + // } + + // template + // BLA::Matrix extractDiag(const BLA::Matrix &x) { + // BLA::Matrix diag; + // for (int i = 0; i < M; i++) { + // diag(i) = x(inds[i], indx[i]); + // } + // return diag; + // } + + // template + // float vecMax(const BLA::Matrix &x) { + // BLA::Matrix diag; + // for (int i = 0; i < M; i++) { + // diag(i) = x(inds[i], indx[i]); + // } + // return diag; + // // TODO fix + // } } diff --git a/src/qmekf.cpp b/src/qmekf.cpp index 9244921..0fb13c5 100644 --- a/src/qmekf.cpp +++ b/src/qmekf.cpp @@ -83,7 +83,7 @@ void StateEstimator::computeInitialOrientation() { } BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float curr_time) { - float dt = curr_time - QuaternionUtils::vecMax(QuaternionUtils::extractSub(lastTimes, {0, 1, 3, 4}));; + // float dt = curr_time - QuaternionUtils::vecMax(QuaternionUtils::extractSub(lastTimes, {0, 1, 3, 4}));; BLA::Matrix<3, 1> unbiased_gyro = gyro; From 63ae7ce331d5b58c1621d9f0e93d65176aa7442d Mon Sep 17 00:00:00 2001 From: 176238837DHH Date: Wed, 3 Dec 2025 18:55:59 -0500 Subject: [PATCH 08/14] new new --- src/QuaternionUtils.cpp | 42 +++++++++++++++++++++++++++++++++++++++-- src/QuaternionUtils.h | 39 +++++++++++--------------------------- src/qmekf.cpp | 42 ++++++++++++++++++++++------------------- 3 files changed, 74 insertions(+), 49 deletions(-) diff --git a/src/QuaternionUtils.cpp b/src/QuaternionUtils.cpp index 1f5d28c..8a6af76 100644 --- a/src/QuaternionUtils.cpp +++ b/src/QuaternionUtils.cpp @@ -3,6 +3,7 @@ #include #include + BLA::Matrix<3, 3> QuaternionUtils::quatToRot(const BLA::Matrix<4, 1> &quat) { float w = quat(0); float x = quat(1); @@ -127,7 +128,8 @@ BLA::Matrix<4, 1> QuaternionUtils::quatMultiply(const BLA::Matrix<4, 1> &p, cons quat(3, 0) = p(0, 0) * q(3, 0) + p(1, 0) * q(2, 0) - p(2, 0) * q(1, 0) + p(3, 0) * q(3, 0); - return quat; + return QuaternionUtils::normalizeQuaternion(quat); + } BLA::Matrix<3, 1> QuaternionUtils::lla2ecef(const BLA::Matrix<3, 1> &lla) { @@ -259,7 +261,7 @@ BLA::Matrix<3,1> QuaternionUtils::lla2ecef(BLA::Matrix<3,1> lla) { } -BLA::Matrix<3, 1> QuaternionUtils::ecef2lla(BLA::Matrix<3, 1> ecef) { +BLA::Matrix<3, 1> ecef2lla(BLA::Matrix<3, 1> ecef) { // Follows this kid idk if right, gotta test, but I'm not writing this bs myself // I think this is's impl not super precise, but not that deep (I think) // https://github.com/kvenkman/ecef2lla/blob/master/ecef2lla.py @@ -299,6 +301,42 @@ BLA::Matrix<3, 1> QuaternionUtils::ecef2lla(BLA::Matrix<3, 1> ecef) { return lla; } +template BLA::Matrix extractSub(const BLA::Matrix &x, const std::array &inds) { + BLA::Matrix sub; + for (int i = 0; i < M; i++) { + sub(i) = x(inds[i]); + } + return sub; + } + +template BLA::Matrix QuaternionUtils::extractDiag(const BLA::Matrix &x) + { + BLA::Matrix diag; + for (int i = 0; i < N; i++) + { + diag(i) = x(i,i); + } + return diag; + } + +template float QuaternionUtils::vecMax(const BLA::Matrix &x) + { + float maxVal = x(0); + for (int i = 1; i < N; i++) + { + if (x(i) > maxVal) { + maxVal = x(i); + } + } + return maxVal; + } + +BLA::Matrix<4, 1> QuaternionUtils::normalizeQuaternion(BLA::Matrix<4, 1> quat) +{ + BLA::Matrix<4, 1> new_quat = quat / BLA::Norm(quat); + return new_quat; +} + diff --git a/src/QuaternionUtils.h b/src/QuaternionUtils.h index 64cc06f..510012b 100644 --- a/src/QuaternionUtils.h +++ b/src/QuaternionUtils.h @@ -59,32 +59,15 @@ namespace QuaternionUtils { BLA::Matrix<3, 1> ecef2lla(BLA::Matrix<3, 1> ecef); - // template - // BLA::Matrix extractSub(const BLA::Matrix &x, - // const std::array &inds) { - // BLA::Matrix sub; - // for (int i = 0; i < M; i++) { - // sub(i) = x(inds[i]); - // } - // return sub; - // } - - // template - // BLA::Matrix extractDiag(const BLA::Matrix &x) { - // BLA::Matrix diag; - // for (int i = 0; i < M; i++) { - // diag(i) = x(inds[i], indx[i]); - // } - // return diag; - // } - - // template - // float vecMax(const BLA::Matrix &x) { - // BLA::Matrix diag; - // for (int i = 0; i < M; i++) { - // diag(i) = x(inds[i], indx[i]); - // } - // return diag; - // // TODO fix - // } + template + BLA::Matrix extractSub(const BLA::Matrix &x, + const std::array &inds); + + template + BLA::Matrix extractDiag(const BLA::Matrix &x); + + template + float vecMax(const BLA::Matrix &x); + + BLA::Matrix<4, 1> normalizeQuaternion(BLA::Matrix<4, 1> quat); } diff --git a/src/qmekf.cpp b/src/qmekf.cpp index 0fb13c5..7867e8c 100644 --- a/src/qmekf.cpp +++ b/src/qmekf.cpp @@ -6,6 +6,8 @@ #include "QuaternionUtils.h" +using namespace QuaternionUtils; + void StateEstimator::init(BLA::Matrix<3, 1> ECEF, float curr_time) { if (ECEF(0) != 0) { launch_ecef = ECEF; @@ -44,7 +46,7 @@ void StateEstimator::init(BLA::Matrix<3, 1> ECEF, float curr_time) { P(idx, idx) = powf(0.1f, 2); } - launch_dcmned2ecef = QuaternionUtils::dcm_ned2ecef(QuaternionUtils::ecef2lla(launch_ecef)); + launch_dcmned2ecef = dcm_ned2ecef(ecef2lla(launch_ecef)); numLoop = 0; sumAccel = {0, 0, 0}; @@ -69,11 +71,11 @@ void StateEstimator::padLoop(BLA::Matrix<3, 1> accel, BLA::Matrix<3, 1> mag, BLA } void StateEstimator::computeInitialOrientation() { - BLA::Matrix<3, 1> normal_i = QuaternionUtils::normal_i_ecef(launch_dcmned2ecef); - BLA::Matrix<3, 1> m_i = QuaternionUtils::m_i_ecef(launch_dcmned2ecef); + BLA::Matrix<3, 1> normal_i = normal_i_ecef(launch_dcmned2ecef); + BLA::Matrix<3, 1> m_i = m_i_ecef(launch_dcmned2ecef); BLA::Matrix<3, 1> normal_b = sumAccel / numLoop; BLA::Matrix<3, 1> m_b = sumMag / numLoop; - BLA::Matrix initial_quat = QuaternionUtils::triad_BE(normal_b, m_b, normal_i, m_i); + BLA::Matrix initial_quat = triad_BE(normal_b, m_b, normal_i, m_i); x(0) = initial_quat(0); @@ -82,8 +84,10 @@ void StateEstimator::computeInitialOrientation() { x(3) = initial_quat(3); } -BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float curr_time) { - // float dt = curr_time - QuaternionUtils::vecMax(QuaternionUtils::extractSub(lastTimes, {0, 1, 3, 4}));; +BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float curr_time) +{ + std::array uhh = {0, 1, 3, 4}; + //float dt = curr_time - vecMax(extractSub(lastTimes, uhh)); BLA::Matrix<3, 1> unbiased_gyro = gyro; @@ -162,7 +166,7 @@ BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float cur // } // if (run_gps_update) { -// BLA::Matrix<3,1> gps_ecef = QuaternionUtils::lla2ecef(gpsData); +// BLA::Matrix<3,1> gps_ecef = lla2ecef(gpsData); // runGPSUpdate(x, gps_ecef); // lastTimes(3) = millis(); // } @@ -205,12 +209,12 @@ BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float cur // x(QMEKFInds::q_y), // x(QMEKFInds::q_z), // }; -// q = QuaternionUtils::quatMultiply(q, dq); +// q = quatMultiply(q, dq); // BLA::Matrix<4,1> qNorm = {q(0) / BLA::Norm(q), q(1) / BLA::Norm(q), q(2) / BLA::Norm(q), q(3) / BLA::Norm(q)}; -// BLA::Matrix<3,1> v_dot = QuaternionUtils::quatToRot(q) * accel + g_i; +// BLA::Matrix<3,1> v_dot = quatToRot(q) * accel + g_i; // BLA::Matrix<3,1> old_v = { // x(QMEKFInds::v_x), x(QMEKFInds::v_y), x(QMEKFInds::v_z) // }; @@ -233,8 +237,8 @@ BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float cur // } // BLA::Matrix<19, 19> StateEstimator::predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt) { -// BLA::Matrix<3,3> gyroSkew = QuaternionUtils::skewSymmetric(gyroVec); -// BLA::Matrix<3,3> accelSkew = QuaternionUtils::skewSymmetric(accelVec); +// BLA::Matrix<3,3> gyroSkew = skewSymmetric(gyroVec); +// BLA::Matrix<3,3> accelSkew = skewSymmetric(accelVec); // BLA::Matrix<4, 1> q = // { @@ -244,7 +248,7 @@ BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float cur // x(QMEKFInds::q_z) // }; -// BLA::Matrix<3, 3> rotMatrix = QuaternionUtils::quatToRot(q); +// BLA::Matrix<3, 3> rotMatrix = quatToRot(q); // BLA::Matrix<19, 19> F; // F.Fill(0); @@ -336,12 +340,12 @@ BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float cur // BLA::Matrix<3, 19> H_accel; // H_accel.Fill(0); -// H_accel.Submatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * (-1.0f * normal_i)); -// H_accel.Submatrix<3, 3>(0, QMEKFInds::ab_x - 1) = -1.0f * QuaternionUtils::quat2DCM(q); +// H_accel.Submatrix<3, 3>(0, 0) = skewSymmetric(quat2DCM(q) * (-1.0f * normal_i)); +// H_accel.Submatrix<3, 3>(0, QMEKFInds::ab_x - 1) = -1.0f * quat2DCM(q); // BLA::Matrix<3, 1> h_accel; -// h_accel = QuaternionUtils::quat2DCM(q) * normal_i; +// h_accel = quat2DCM(q) * normal_i; // EKFCalcErrorInject(x, P, accel_meas, H_accel, h_accel, R_accel); @@ -359,19 +363,19 @@ BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float cur // BLA::Matrix<3, 19> H_mag; // H_mag.Fill(0); -// H_mag.Submatrix<3, 3>(0, 0) = QuaternionUtils::skewSymmetric(QuaternionUtils::quat2DCM(q) * m_i); +// H_mag.Submatrix<3, 3>(0, 0) = skewSymmetric(quat2DCM(q) * m_i); // H_mag.Submatrix<3, 3>(0, QMEKFInds::gb_x) = I_3; // BLA::Matrix<3, 1> h_mag; -// h_mag = QuaternionUtils::quat2DCM(q) * m_i; +// h_mag = quat2DCM(q) * m_i; // EKFCalcErrorInject(x, P, mag_meas, H_mag, h_mag, R_mag); // } // BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps_meas_ecef) { -// BLA::Matrix<3, 1> pos_ned = QuaternionUtils::ecef2ned(gps_meas_ecef, launch_ecef, R_ET); +// BLA::Matrix<3, 1> pos_ned = ecef2ned(gps_meas_ecef, launch_ecef, R_ET); // BLA::Matrix<3, 19> H_gps; // H_gps.Fill(0); @@ -419,7 +423,7 @@ BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float cur // x(2), // x(3) // }; -// BLA::Matrix<4, 1> q = QuaternionUtils::quatMultiply(old_q, dq); +// BLA::Matrix<4, 1> q = quatMultiply(old_q, dq); // // Set quats // x(0) = q(0); From 8d4d2602092f1d419acaffcef93813e5537bd4ee Mon Sep 17 00:00:00 2001 From: Abhay Mathur <66543532+abhayma1000@users.noreply.github.com> Date: Thu, 4 Dec 2025 18:12:39 -0500 Subject: [PATCH 09/14] Finished base EKF writing --- .gitignore | 5 +- .vscode/c_cpp_properties.json | 25 -- src/Conventions.txt | 7 +- src/QuaternionUtils.cpp | 43 +-- src/QuaternionUtils.h | 71 ++++- src/kfConsts.h | 8 + src/qmekf.cpp | 565 ++++++++++++++-------------------- src/qmekf.h | 81 +++-- 8 files changed, 345 insertions(+), 460 deletions(-) delete mode 100644 .vscode/c_cpp_properties.json diff --git a/.gitignore b/.gitignore index 89cc49c..2089f94 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,2 @@ .pio -.vscode/.browse.c_cpp.db* -.vscode/c_cpp_properties.json -.vscode/launch.json -.vscode/ipch +.vscode/* diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index a75e91e..0000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,25 +0,0 @@ -{ - "configurations": [ - { - "name": "Win32", - "includePath": [ - "${workspaceFolder}/**", - "C:/Users/krizz/OneDrive/Documents/PlatformIO/Projects/QMEKF-Script/.pio/libdeps/disco_h747xi/BasicLinearAlgebra", - "C:/Users/krizz/OneDrive/Documents/HPRC", - "C:/Users/krizz/.platformio/packages/framework-arduinoststm32/cores/arduino", - "C:/Users/krizz/.platformio/packages/framework-arduinoststm32/libraries/SrcWrapper/inc", - "C:/Users/krizz/.platformio/packages/framework-arduinoststm32/libraries/SrcWrapper/inc/LL" - ], - "defines": [ - "_DEBUG", - "UNICODE", - "_UNICODE" - ], - "compilerPath": "C:\\msys64\\ucrt64\\bin\\gcc.exe", - "cStandard": "c17", - "cppStandard": "gnu++17", - "intelliSenseMode": "windows-gcc-x64" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/src/Conventions.txt b/src/Conventions.txt index 44fc70f..6a634ec 100644 --- a/src/Conventions.txt +++ b/src/Conventions.txt @@ -12,7 +12,8 @@ I change take in int variable state to onLoop, not the boolean: Frequencies variable in format: - priori step + gyro integrate + accel integrate accel upate mag update gps update @@ -28,4 +29,6 @@ BLA library: * BLA::CrossProduct(A, B); * Define: BLA::Matrix<2, 3> x = {1, 2, 3, 4, 5, 6,} -* Multiplication, addition is just * and + +* Multiplication, addition is just *, +, / (note: ensure by a float. ex: 2.0f) +* Submatrix: A.Submatrix<2,2>(1,0); Of a 3x2 mat A, returns bottom 2 rows + diff --git a/src/QuaternionUtils.cpp b/src/QuaternionUtils.cpp index 8a6af76..1b44b57 100644 --- a/src/QuaternionUtils.cpp +++ b/src/QuaternionUtils.cpp @@ -261,7 +261,7 @@ BLA::Matrix<3,1> QuaternionUtils::lla2ecef(BLA::Matrix<3,1> lla) { } -BLA::Matrix<3, 1> ecef2lla(BLA::Matrix<3, 1> ecef) { +BLA::Matrix<3, 1> QuaternionUtils::ecef2lla(BLA::Matrix<3, 1> ecef) { // Follows this kid idk if right, gotta test, but I'm not writing this bs myself // I think this is's impl not super precise, but not that deep (I think) // https://github.com/kvenkman/ecef2lla/blob/master/ecef2lla.py @@ -301,42 +301,19 @@ BLA::Matrix<3, 1> ecef2lla(BLA::Matrix<3, 1> ecef) { return lla; } -template BLA::Matrix extractSub(const BLA::Matrix &x, const std::array &inds) { - BLA::Matrix sub; - for (int i = 0; i < M; i++) { - sub(i) = x(inds[i]); - } - return sub; - } - -template BLA::Matrix QuaternionUtils::extractDiag(const BLA::Matrix &x) - { - BLA::Matrix diag; - for (int i = 0; i < N; i++) - { - diag(i) = x(i,i); - } - return diag; - } - -template float QuaternionUtils::vecMax(const BLA::Matrix &x) - { - float maxVal = x(0); - for (int i = 1; i < N; i++) - { - if (x(i) > maxVal) { - maxVal = x(i); - } - } - return maxVal; - } - -BLA::Matrix<4, 1> QuaternionUtils::normalizeQuaternion(BLA::Matrix<4, 1> quat) -{ +BLA::Matrix<4, 1> QuaternionUtils::normalizeQuaternion(BLA::Matrix<4, 1> quat) { BLA::Matrix<4, 1> new_quat = quat / BLA::Norm(quat); return new_quat; } +float QuaternionUtils::cosd(float rads) { + float pi = 3.141592653589; + return cos(rads * (180.0f / pi)); +} +float QuaternionUtils::sind(float rads) { + float pi = 3.141592653589; + return sin(rads * (180.0f / pi)); +} diff --git a/src/QuaternionUtils.h b/src/QuaternionUtils.h index 510012b..ab10061 100644 --- a/src/QuaternionUtils.h +++ b/src/QuaternionUtils.h @@ -1,6 +1,7 @@ #pragma once #include "BasicLinearAlgebra.h" +#include namespace QuaternionUtils { // Convert quaternion (w,x,y,z) to rotation matrix @@ -59,15 +60,67 @@ namespace QuaternionUtils { BLA::Matrix<3, 1> ecef2lla(BLA::Matrix<3, 1> ecef); - template - BLA::Matrix extractSub(const BLA::Matrix &x, - const std::array &inds); - - template - BLA::Matrix extractDiag(const BLA::Matrix &x); - - template - float vecMax(const BLA::Matrix &x); + template + BLA::Matrix extractSub(const BLA::Matrix &x, const BLA::Matrix inds) { + BLA::Matrix sub; + for (size_t i = 0; i < M; i++) { + sub(i) = x(inds(i, 0)); + } + return sub; + } + + template + BLA::Matrix extractDiag(const BLA::Matrix &x) { + BLA::Matrix diag; + for (int i = 0; i < N; i++) { + diag(i) = x(i,i); + } + return diag; + } + + template + BLA::Matrix toDiag(const BLA::Matrix x) { + BLA::Matrix diag; + diag.Fill(0.0f); + for (int i = 0; i < N; i++) { + diag(i, i) = x(i, 0); + } + return diag; + } + + template + float vecMax(const BLA::Matrix &x) { + float maxVal = x(0, 0); + for (int i = 0; i < N; i++) { + if (x(i, 0) > maxVal) { + maxVal = x(i, 0); + } + } + return maxVal; + } + + template + BLA::Matrix setSub(BLA::Matrix &x, const BLA::Matrix inds, const BLA::Matrix vals) { + for (int i = 0; i < M; i++) { + x(inds(i, 0), 0) = vals(i, 0); + } + return x; + } BLA::Matrix<4, 1> normalizeQuaternion(BLA::Matrix<4, 1> quat); + + float cosd(float rads); + float sind(float rads); + + template + BLA::Matrix stack(BLA::Matrix x, BLA::Matrix y) { + BLA::Matrix ret; + for (int i = 0; i < N; i++) { + ret(i, 0) = x(i, 0); + } + for (int i = 0; i < M; i++) { + ret(N + i, 0) = y(i, 0); + } + return ret; + } } diff --git a/src/kfConsts.h b/src/kfConsts.h index c51455d..c7668b8 100644 --- a/src/kfConsts.h +++ b/src/kfConsts.h @@ -24,6 +24,14 @@ BLA::Matrix<3, 1> gyro_var = {square(0.0947f), square(0.0947f), square(0.0947f)} namespace lps22_const { BLA::Matrix<1, 1> baro_var = {0.0f}; // uh what, why is it 0 BLA::Matrix<1, 1> baro_bias = {0.0f}; + +constexpr float P0 = 101325.0; // [P] trad sea level +constexpr float L = -0.0065; +constexpr float T0 = 288.15; +constexpr float M = 0.0289644; +constexpr float R = 8.3144598; +constexpr float g_e = 9.80665; // [m/s^2] + }; namespace icm20948_const { diff --git a/src/qmekf.cpp b/src/qmekf.cpp index 7867e8c..4a2eb8b 100644 --- a/src/qmekf.cpp +++ b/src/qmekf.cpp @@ -6,6 +6,8 @@ #include "QuaternionUtils.h" +#include + using namespace QuaternionUtils; void StateEstimator::init(BLA::Matrix<3, 1> ECEF, float curr_time) { @@ -24,27 +26,27 @@ void StateEstimator::init(BLA::Matrix<3, 1> ECEF, float curr_time) { // TODO idk figure this out P.Fill(0.0f); - for(uint8_t idx : QMEKFInds::quat) { - P(idx, idx) = 1e-8; + for(int i = 0 ; i < QMEKFInds::quat.Rows; i++) { + P(QMEKFInds::quat(i, 0), QMEKFInds::quat(i, 0)) = 1e-8; + } + for(int i = 0 ; i < QMEKFInds::vel.Rows; i++) { + P(QMEKFInds::vel(i, 0), QMEKFInds::vel(i, 0)) = 1e-8; + } + for(int i = 0 ; i < QMEKFInds::pos.Rows; i++) { + P(QMEKFInds::pos(i, 0), QMEKFInds::pos(i, 0)) = 1e-8; } - for (uint8_t idx : QMEKFInds::vel) { - P(idx, idx) = 1e-8; - } - for (uint8_t idx : QMEKFInds::pos) { - P(idx, idx) = 1e-8; - } - for(uint8_t idx : QMEKFInds::gyroBias) { - P(idx, idx) = powf(icm20948_const::gyro_VRW, 2.0f); + for(int i = 0 ; i < QMEKFInds::gyroBias.Rows; i++) { + P(QMEKFInds::gyroBias(i, 0), QMEKFInds::gyroBias(i, 0)) = powf(icm20948_const::gyro_VRW, 2.0f); } - for(uint8_t idx: QMEKFInds::accelBias) { - P(idx, idx) = powf(icm20948_const::accelXY_VRW, 2.0f); + for(int i = 0 ; i < QMEKFInds::accelBias.Rows; i++) { + P(QMEKFInds::accelBias(i, 0), QMEKFInds::accelBias(i, 0)) = powf(icm20948_const::accelXY_VRW, 2.0f); } - for(uint8_t idx : QMEKFInds::magBias) { - P(idx, idx) = powf(0.1f, 2); + for(int i = 0 ; i < QMEKFInds::magBias.Rows; i++) { + P(QMEKFInds::magBias(i, 0), QMEKFInds::magBias(i, 0)) = powf(0.1f, 2); + } + for(int i = 0 ; i < QMEKFInds::baroBias.Rows; i++) { + P(QMEKFInds::baroBias(i, 0), QMEKFInds::baroBias(i, 0)) = powf(0.1f, 2); } - for (uint8_t idx : QMEKFInds::baroBias) { - P(idx, idx) = powf(0.1f, 2); - } launch_dcmned2ecef = dcm_ned2ecef(ecef2lla(launch_ecef)); @@ -84,381 +86,260 @@ void StateEstimator::computeInitialOrientation() { x(3) = initial_quat(3); } -BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float curr_time) -{ - std::array uhh = {0, 1, 3, 4}; - //float dt = curr_time - vecMax(extractSub(lastTimes, uhh)); - BLA::Matrix<3, 1> unbiased_gyro = gyro; +BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float curr_time) { + BLA::Matrix<4, 1> last_relevant_times = {0, 1, 3, 4}; + float dt = curr_time - vecMax(extractSub(lastTimes, last_relevant_times)); + // TODO combine gyro through vimu + BLA::Matrix<3, 1> unbiased_gyro = gyro - extractSub(x, QMEKFInds::gyroBias); + BLA::Matrix<4, 1> new_q = quatMultiply(extractSub(x, QMEKFInds::quat), rotVec2Quat(unbiased_gyro)); + x = setSub(x, QMEKFInds::quat, new_q); gyro_prev = unbiased_gyro; - + lastTimes(0) = curr_time; return x; } +BLA::Matrix<20, 1> StateEstimator::fastAccelProp(BLA::Matrix<3, 1> accel, float curr_time) { + BLA::Matrix<4, 1> last_relevant_times = (1, 4, 5); + float dt = curr_time - vecMax(extractSub(lastTimes, last_relevant_times)); -// BLA::Matrix<20,1> StateEstimator::onLoop(int state) { -// // Read data from sensors and convert values - -// float dt; + // TODO combine accel through vimu -// float gyrX = IMUData->gyrX; -// float gyrY = IMUData->gyrY; -// float gyrZ = IMUData->gyrZ; + BLA::Matrix<3, 1> unbiased_accel = accel - extractSub(x, QMEKFInds::accelBias); + BLA::Matrix<3, 3> q_conj_dcm = quat2DCM(quatConjugate((x, QMEKFInds::quat))); + BLA::Matrix<3, 1> v = ((q_conj_dcm * unbiased_accel + q_conj_dcm * accel_prev) / 2.0f) + g_i_ecef(launch_dcmned2ecef) * dt + vel_prev; + BLA::Matrix<3, 1> p = ((v + vel_prev) / 2.0f) * dt + pos_prev; + x = setSub(x, QMEKFInds::vel, v); + x = setSub(x, QMEKFInds::pos, p); -// float aclX = IMUData->accelX; -// float aclY = IMUData->accelY; -// float aclZ = IMUData->accelZ; + accel_prev = unbiased_accel; + vel_prev = v; + pos_prev = p; + lastTimes(1) = curr_time; + return x; +} -// float magX = IMUData->magX; -// float magY = IMUData->magY; -// float magZ = IMUData->magZ; - - -// // TODO get in the GPS data and baro data from somewhere -// BLA::Matrix<3,1> gyro = {gyrX, gyrY, gyrZ}; // [rad/s] -// BLA::Matrix<3,1> accel = {aclX, aclY, aclZ}; // [m/s^s] -// BLA::Matrix<3,1> mag = {magX, magY, magZ}; // [uT] -// BLA::Matrix<3,1> gpsData = {gpsData(0), gpsData(1), gpsData(2)}; -// // BLA::Matrix<1,1> baro = {baroZ}; - -// // Remove biases from each measurement -// BLA::Matrix<3,1> unbiased_gyro = {gyro(0) - x(QMEKFInds::gb_x), gyro(1) - x(QMEKFInds::gb_y), gyro(2) - x(QMEKFInds::gb_z)}; -// BLA::Matrix<3,1> unbiased_accel = {accel(0) - x(QMEKFInds::ab_x), accel(1) - x(QMEKFInds::ab_y), accel(2) - x(QMEKFInds::ab_z)}; -// BLA::Matrix<3,1> unbiased_mag = {mag(0) - x(QMEKFInds::mb_x), mag(1) - x(QMEKFInds::mb_y), mag(2) - x(QMEKFInds::mb_z)}; -// // BLA::Matrix<1,1> unbiased_baro = {baro(0) - x(20)}; - - -// float time = millis(); -// bool run_priori = time - lastTimes(0) >= frequencies(0); -// bool run_accel_update = time - lastTimes(1) >= frequencies(1); -// bool run_mag_update = time - lastTimes(2) >= frequencies(2); -// bool run_gps_update = time - lastTimes(3) >= frequencies(3); -// // bool run_baro_update = time - lastTimeBaro(4) >= frequencies(4); - -// if(run_priori) { -// // TODO eventually implement RK4 here, but I don't understand it yet -// float lastAttUpdate = max(max(lastTimes(0), lastTimes(1)), lastTimes(2)); // Maximum of the lastTimes(0, 1, 2) -// float lastPVUpdate = max(max(lastTimes(0), lastTimes(3)), lastTimes(4)); // Maximum of the lastTimes(0, 3, 4) -// float dt_att = millis() - lastAttUpdate; -// float dt_pv = millis() - lastPVUpdate; - -// x = fastIMUProp(gyro, accel, dt_att, dt_pv); - - -// lastTimes(0) = millis(); -// } - -// if (run_accel_update || run_mag_update || run_gps_update) { -// dt = millis() - max(max(lastTimes(0), lastTimes(1)), max(lastTimes(2), lastTimes(3)));; -// P = predictionFunction(P, gyro, accel, dt); -// } - -// if (run_accel_update) { -// runAccelUpdate(x, accel); -// lastTimes(1) = millis(); -// } - -// if (run_mag_update) { -// runMagUpdate(x, mag); -// lastTimes(2) = millis(); -// } - -// if (run_gps_update) { -// BLA::Matrix<3,1> gps_ecef = lla2ecef(gpsData); -// runGPSUpdate(x, gps_ecef); -// lastTimes(3) = millis(); -// } - -// // if (run_baro_update) { -// // run_baro_update(); -// // lastTimes(4) = millis(); -// // } - -// //Update sensor readings -// gyro_prev = unbiased_gyro; -// accel_prev = unbiased_accel; -// mag_prev = unbiased_mag; -// gps_prev = gpsData; -// // baro_prev = unbiased_baro; - -// return x; -// } - -// BLA::Matrix<20,1> StateEstimator::fastIMUProp(BLA::Matrix<3,1> gyro, BLA::Matrix<3, 1> accel, float att_dt, float pv_dt) { - -// BLA::Matrix<3, 1> v; -// BLA::Matrix<3, 1> p; - -// // TODO change to from world model when go to ECEF -// BLA::Matrix<3,1> gyro_int = {gyro(0)*att_dt, gyro(1)*att_dt, gyro(2)*att_dt}; -// float rotVecNorm = BLA::Norm(gyro_int); -// BLA::Matrix<3,1> axis = {(gyro_int(0) / rotVecNorm), (gyro_int(1) / rotVecNorm), (gyro_int(2) / rotVecNorm)}; -// BLA::Matrix<4,1> dq = -// { -// cos(rotVecNorm/2.0f), -// axis(0) * sinf(rotVecNorm/2.0f), -// axis(1) * sinf(rotVecNorm/2.0f), -// axis(2) * sinf(rotVecNorm/2.0f), -// }; -// BLA::Matrix<4,1> q = -// { -// x(QMEKFInds::q_w), -// x(QMEKFInds::q_x), -// x(QMEKFInds::q_y), -// x(QMEKFInds::q_z), -// }; -// q = quatMultiply(q, dq); -// BLA::Matrix<4,1> qNorm = {q(0) / BLA::Norm(q), q(1) / BLA::Norm(q), q(2) / BLA::Norm(q), q(3) / BLA::Norm(q)}; - - - -// BLA::Matrix<3,1> v_dot = quatToRot(q) * accel + g_i; -// BLA::Matrix<3,1> old_v = { -// x(QMEKFInds::v_x), x(QMEKFInds::v_y), x(QMEKFInds::v_z) -// }; -// BLA::Matrix<3,1> old_p = { -// x(QMEKFInds::p_x), x(QMEKFInds::p_y), x(QMEKFInds::p_z) -// }; - -// v = old_v + v_dot * pv_dt; -// p = old_p + v * pv_dt; - -// x(QMEKFInds::v_x) = v(0); -// x(QMEKFInds::v_y) = v(1); -// x(QMEKFInds::v_z) = v(2); -// x(QMEKFInds::p_x) = p(0); -// x(QMEKFInds::p_y) = p(1); -// x(QMEKFInds::p_z) = p(2); - -// return x; - -// } - -// BLA::Matrix<19, 19> StateEstimator::predictionFunction(BLA::Matrix<19, 19> P_, BLA::Matrix<3, 1> accelVec, BLA::Matrix<3, 1> gyroVec, float dt) { -// BLA::Matrix<3,3> gyroSkew = skewSymmetric(gyroVec); -// BLA::Matrix<3,3> accelSkew = skewSymmetric(accelVec); - -// BLA::Matrix<4, 1> q = -// { -// x(QMEKFInds::q_w), -// x(QMEKFInds::q_x), -// x(QMEKFInds::q_y), -// x(QMEKFInds::q_z) -// }; - -// BLA::Matrix<3, 3> rotMatrix = quatToRot(q); +BLA::Matrix<19, 19> StateEstimator::ekfPredict(float curr_time) { + BLA::Matrix<4, 1> att_last_relevant_times = {0, 1, 3, 4}; + BLA::Matrix<4, 1> pv_last_relevant_times = (1, 4, 5); + float att_dt = curr_time - vecMax(extractSub(lastTimes, att_last_relevant_times)); + float pv_dt = curr_time - vecMax(extractSub(lastTimes, pv_last_relevant_times)); + + + BLA::Matrix<3,3> gyroSkew = skewSymmetric(gyro_prev); + BLA::Matrix<3,3> accelSkew = skewSymmetric(accel_prev); -// BLA::Matrix<19, 19> F; -// F.Fill(0); + BLA::Matrix<3, 3> q_conj_dcm = quat2DCM(quatConjugate((x, QMEKFInds::quat))); -// //Row 1 - 3 -// F.Submatrix<3, 3>(0, QMEKFInds::q_w) = -1.0f * gyroSkew; -// F.Submatrix<3, 3>(0, QMEKFInds::gb_x) = -1.0f * I_3; + BLA::Matrix<19, 19> F; + F.Fill(0); -// //Row 4 - 6 -// F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::q_w) = -1.0f * rotMatrix * accelSkew; -// F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::ab_x) = -1.0f * rotMatrix; + //Row 1 - 3 + F.Submatrix<3, 3>(0, QMEKFInds::q_w) = -1.0f * gyroSkew; + F.Submatrix<3, 3>(0, QMEKFInds::gb_x - 1) = -1.0f * I_3; -// //Row 7 - 9 -// F.Submatrix<3, 3>(QMEKFInds::p_x -1, 3) = I_3; + //Row 4 - 6 + F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::q_w) = -1.0f * q_conj_dcm * accelSkew; + F.Submatrix<3, 3>(QMEKFInds::v_x - 1, QMEKFInds::ab_x - 1) = -1.0f * q_conj_dcm; + + //Row 7 - 9 + F.Submatrix<3, 3>(QMEKFInds::p_x - 1, QMEKFInds::v_x - 1) = I_3; -// BLA::Matrix<19, 19> phi; -// phi.Fill(0); + BLA::Matrix<19, 19> phi; + phi.Fill(0); -// phi = I_19 + (F * dt) + (0.5f * F * F * float(pow(dt, 2))); + phi = I_19 + (F * pv_dt) + (0.5f * F * F * float(pow(pv_dt, 2))); // I was hoping to split up the dt. Not that deep tho -// BLA::Matrix<19, 19> phi_t = ~phi; + BLA::Matrix<19, 19> phi_t = ~phi; -// BLA::Matrix<19, 19> Q_d; -// Q_d.Fill(0); + // Process noise + BLA::Matrix<19, 19> Q_d; + Q_d.Fill(0); -// BLA::Matrix<3, 3> gyro_var_diag; -// gyro_var_diag.Fill(0); -// gyro_var_diag(0, 0) = QMEKFInds::gyro_var; -// gyro_var_diag(1, 1) = QMEKFInds::gyro_var; -// gyro_var_diag(2, 2) = QMEKFInds::gyro_var; + // TODO this part eventually + // BLA::Matrix<3, 3> gyro_var_diag; + // gyro_var_diag.Fill(0); + // gyro_var_diag(0, 0) = QMEKFInds::gyro_var; + // gyro_var_diag(1, 1) = QMEKFInds::gyro_var; + // gyro_var_diag(2, 2) = QMEKFInds::gyro_var; -// BLA::Matrix<3, 3> gyro_bias_var_diag; -// gyro_bias_var_diag.Fill(0); -// gyro_bias_var_diag(0, 0) = QMEKFInds::gyro_bias_var; -// gyro_bias_var_diag(1, 1) = QMEKFInds::gyro_bias_var; -// gyro_bias_var_diag(2, 2) = QMEKFInds::gyro_bias_var; + // BLA::Matrix<3, 3> gyro_bias_var_diag; + // gyro_bias_var_diag.Fill(0); + // gyro_bias_var_diag(0, 0) = QMEKFInds::gyro_bias_var; + // gyro_bias_var_diag(1, 1) = QMEKFInds::gyro_bias_var; + // gyro_bias_var_diag(2, 2) = QMEKFInds::gyro_bias_var; -// BLA::Matrix<3, 3> accel_bias_var_diag; -// accel_bias_var_diag.Fill(0); -// accel_bias_var_diag(0, 0) = QMEKFInds::accel_bias_var; -// accel_bias_var_diag(1, 1) = QMEKFInds::accel_bias_var; -// accel_bias_var_diag(2, 2) = QMEKFInds::accel_bias_var; + // BLA::Matrix<3, 3> accel_var_diag; + // BLA::Matrix<3, 3> accel_bias_var_diag; + // accel_bias_var_diag.Fill(0); + // accel_bias_var_diag(0, 0) = QMEKFInds::accel_bias_var; + // accel_bias_var_diag(1, 1) = QMEKFInds::accel_bias_var; + // accel_bias_var_diag(2, 2) = QMEKFInds::accel_bias_var; -// Q_d.Submatrix<3, 3>(QMEKFInds::q_w, QMEKFInds::q_w) = (gyro_var_diag * dt) + (gyro_bias_var_diag * float((pow(dt, 3) / 10))); -// Q_d.Submatrix<3, 3>(QMEKFInds::q_w, 9) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2)); + // BLA::Matrix<3, 3> mag_var_diag; + // BLA::Matrix<1, 1> baro_var_diag; -// BLA::Matrix<3, 3> R_grav_diag; -// R_grav_diag.Fill(0); -// R_grav_diag(0, 0) = QMEKFInds::R_grav; -// R_grav_diag(1, 1) = QMEKFInds::R_grav; -// R_grav_diag(2, 2) = QMEKFInds::R_grav; + // Q_d.Submatrix<3, 3>(QMEKFInds::q_w, QMEKFInds::q_w) = (gyro_var_diag * dt) + (gyro_bias_var_diag * float((pow(dt, 3) / 10))); + // Q_d.Submatrix<3, 3>(QMEKFInds::q_w, 9) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2)); -// Q_d.Submatrix<3 ,3>(3, 3) = R_grav_diag * dt + accel_bias_var_diag * float((pow(dt, 3) / 3)); -// Q_d.Submatrix<3, 3>(3, 6) = accel_bias_var_diag * float((pow(dt ,4) / 8.0)) + R_grav_diag * float((pow(dt, 2) / 2.0)); -// Q_d.Submatrix<3, 3>(3, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); + // Q_d.Submatrix<3 ,3>(3, 3) = R_grav_diag * dt + accel_bias_var_diag * float((pow(dt, 3) / 3)); + // Q_d.Submatrix<3, 3>(3, 6) = accel_bias_var_diag * float((pow(dt ,4) / 8.0)) + R_grav_diag * float((pow(dt, 2) / 2.0)); + // Q_d.Submatrix<3, 3>(3, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); -// Q_d.Submatrix<3, 3>(6, 3) = R_grav_diag * float((pow(dt, 2) / 2)) + accel_bias_var_diag * float((pow(dt, 4) / 8.0)); -// Q_d.Submatrix<3, 3>(6, 6) = R_grav_diag * float((pow(dt, 3) / 3.0)) + accel_bias_var_diag * float((pow(dt, 5) / 20.0)); -// Q_d.Submatrix<3, 3>(6, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 3) / 6.0)); + // Q_d.Submatrix<3, 3>(6, 3) = R_grav_diag * float((pow(dt, 2) / 2)) + accel_bias_var_diag * float((pow(dt, 4) / 8.0)); + // Q_d.Submatrix<3, 3>(6, 6) = R_grav_diag * float((pow(dt, 3) / 3.0)) + accel_bias_var_diag * float((pow(dt, 5) / 20.0)); + // Q_d.Submatrix<3, 3>(6, 10) = -1.0f * accel_bias_var_diag * float((pow(dt, 3) / 6.0)); -// Q_d.Submatrix<3, 3>(9, 0) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); -// Q_d.Submatrix<3, 3>(9, 9) = gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); + // Q_d.Submatrix<3, 3>(9, 0) = -1.0f * gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); + // Q_d.Submatrix<3, 3>(9, 9) = gyro_bias_var_diag * float((pow(dt, 2) / 2.0)); -// Q_d.Submatrix<3, 3>(12, 3) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); -// Q_d.Submatrix<3, 3>(12, 6) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); -// Q_d.Submatrix<3, 3>(12, 12) = accel_bias_var_diag * dt; + // Q_d.Submatrix<3, 3>(12, 3) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); + // Q_d.Submatrix<3, 3>(12, 6) = -1.0f * accel_bias_var_diag * float((pow(dt, 2) / 2.0)); + // Q_d.Submatrix<3, 3>(12, 12) = accel_bias_var_diag * dt; -// Q_d(15, 15) = QMEKFInds::mag_bias_var * dt; + // Q_d(15, 15) = QMEKFInds::mag_bias_var * dt; -// Q_d(18, 18) = QMEKFInds::baro_bias_var * dt; + // Q_d(18, 18) = QMEKFInds::baro_bias_var * dt; -// BLA::Matrix<19, 19> P; + // BLA::Matrix<19, 19> P; -// P = phi * P_ * phi_t + Q_d; + P = phi * P * phi_t + Q_d; -// return P; + return P; + +} -// } +BLA::Matrix<20, 1> StateEstimator::runAccelUpdate(BLA::Matrix<3, 1> a_b, float curr_time) { + BLA::Matrix<3, 1> unbiased_accel = a_b - extractSub(x, QMEKFInds::accelBias); -// BLA::Matrix<20, 1> StateEstimator::runAccelUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3,1> accel_meas) -// { -// BLA::Matrix<4,1> q = -// { -// x(QMEKFInds::q_w), -// x(QMEKFInds::q_x), -// x(QMEKFInds::q_y), -// x(QMEKFInds::q_z), -// }; + BLA::Matrix<4,1> q = extractSub(x, QMEKFInds::quat); -// BLA::Matrix<3, 19> H_accel; -// H_accel.Fill(0); -// H_accel.Submatrix<3, 3>(0, 0) = skewSymmetric(quat2DCM(q) * (-1.0f * normal_i)); -// H_accel.Submatrix<3, 3>(0, QMEKFInds::ab_x - 1) = -1.0f * quat2DCM(q); + BLA::Matrix<3, 1> h_accel = quat2DCM(q) * normal_i_ecef(launch_dcmned2ecef); -// BLA::Matrix<3, 1> h_accel; + BLA::Matrix<3, 19> H_accel; + H_accel.Fill(0); + H_accel.Submatrix<3, 3>(0, 0) = skewSymmetric(h_accel); + H_accel.Submatrix<3, 3>(0, QMEKFInds::ab_x - 1) = I_3; -// h_accel = quat2DCM(q) * normal_i; + BLA::Matrix<3, 3> R = toDiag(icm20948_const::accel_var); -// EKFCalcErrorInject(x, P, accel_meas, H_accel, h_accel, R_accel); - -// } + lastTimes(2) = curr_time; + return ekfCalcErrorInject(unbiased_accel, H_accel, h_accel, R); +} + +BLA::Matrix<20, 1> StateEstimator::runMagUpdate(BLA::Matrix<3, 1> m_b, float curr_time) { + BLA::Matrix<3, 1> unbiased_accel = m_b - extractSub(x, QMEKFInds::magBias); -// BLA::Matrix<20, 1> StateEstimator::runMagUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> mag_meas) { -// // TODO input igrm model somehow figure out -// BLA::Matrix<4, 1> q = -// { -// x(QMEKFInds::q_w), -// x(QMEKFInds::q_x), -// x(QMEKFInds::q_y), -// x(QMEKFInds::q_z) -// }; + BLA::Matrix<4,1> q = extractSub(x, QMEKFInds::quat); -// BLA::Matrix<3, 19> H_mag; -// H_mag.Fill(0); -// H_mag.Submatrix<3, 3>(0, 0) = skewSymmetric(quat2DCM(q) * m_i); -// H_mag.Submatrix<3, 3>(0, QMEKFInds::gb_x) = I_3; + BLA::Matrix<3, 1> h_mag = quat2DCM(q) * m_i_ecef(launch_dcmned2ecef); -// BLA::Matrix<3, 1> h_mag; + BLA::Matrix<3, 19> H_mag; + H_mag.Fill(0); + H_mag.Submatrix<3, 3>(0, 0) = skewSymmetric(h_mag); + H_mag.Submatrix<3, 3>(0, QMEKFInds::mb_x - 1) = I_3; -// h_mag = quat2DCM(q) * m_i; + BLA::Matrix<3, 3> R = toDiag(icm20948_const::mag_var); -// EKFCalcErrorInject(x, P, mag_meas, H_mag, h_mag, R_mag); + lastTimes(3) = curr_time; + return ekfCalcErrorInject(unbiased_accel, H_mag, h_mag, R); +} + +BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<3, 1> gpsPos, BLA::Matrix<3, 1> gpsVel, bool velOrientation, float curr_time) { + lastTimes(4) = curr_time; + if (velOrientation) { + BLA::Matrix<9, 1> combined_sens = stack(stack(gpsVel, gpsPos), gpsVel); + BLA::Matrix<3, 1> v_b = {BLA::Norm(gpsVel), 0, 0}; + BLA::Matrix<4, 1> q = extractSub(x, QMEKFInds::quat); + BLA::Matrix<3, 1> h_vi = quat2DCM(quatConjugate(q)) * v_b; + BLA::Matrix<9, 1> h_gps = stack(extractSub(x, stack(QMEKFInds::vel, QMEKFInds::pos)), extractSub(x, QMEKFInds::vel)); + + BLA::Matrix<9, 19> H_gps; + H_gps.Fill(0); + H_gps.Submatrix<3, 3>(0, QMEKFInds::v_x - 1) = I_3; + H_gps.Submatrix<3, 3>(3, QMEKFInds::p_x - 1) = I_3; + H_gps.Submatrix<3, 3>(6, 0) = -1.0f * quat2DCM(quatConjugate(q)) * skewSymmetric(v_b); + + BLA::Matrix<9, 9> R = toDiag(stack(stack(Max10S_const::gpsVel_var, Max10S_const::gpsPos_var), Max10S_const::gpsVel_var)); + + return ekfCalcErrorInject(combined_sens, H_gps, h_gps, R); + } else { + BLA::Matrix<6, 1> combined_sens = stack(gpsVel, gpsPos); + BLA::Matrix<6, 19> H_gps; + H_gps.Fill(0); + H_gps.Submatrix<3, 3>(0, QMEKFInds::v_x - 1) = I_3; + H_gps.Submatrix<3, 3>(3, QMEKFInds::p_x - 1) = I_3; + + BLA::Matrix<6, 1> h_gps = extractSub(x, stack(QMEKFInds::vel, QMEKFInds::pos)); + + BLA::Matrix<6, 6> R = toDiag(stack(Max10S_const::gpsVel_var, Max10S_const::gpsPos_var)); + return ekfCalcErrorInject(combined_sens, H_gps, h_gps, R); + } -// } +} -// BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<20, 1> &x, BLA::Matrix<3, 1> gps_meas_ecef) { -// BLA::Matrix<3, 1> pos_ned = ecef2ned(gps_meas_ecef, launch_ecef, R_ET); +BLA::Matrix<20, 1> StateEstimator::runBaroUpdate(BLA::Matrix<1, 1> baro, float curr_time) { -// BLA::Matrix<3, 19> H_gps; -// H_gps.Fill(0); -// H_gps.Submatrix<3, 3>(0, QMEKFInds::p_x) = I_3; + BLA::Matrix<3, 1> lla = ecef2lla(extractSub(x, QMEKFInds::pos)); -// BLA::Matrix<3, 1> h_gps = { -// x(QMEKFInds::p_x), -// x(QMEKFInds::p_y), -// x(QMEKFInds::p_z), -// }; + BLA::Matrix<1, 1> h_baro = {std::pow(lps22_const::P0 * (1.0f + (lps22_const::L * lla(2))) / lps22_const::T0, -1.0f * lps22_const::g_e * lps22_const::M / (lps22_const::R * lps22_const::L))}; -// EKFCalcErrorInject(x, P, pos_ned, H_gps, h_gps, R_gps); - -// } + float dP_dh = (-1.0f * lps22_const::g_e * h_baro(0, 0) * lps22_const::M) / (lps22_const::R * (lps22_const::T0 + lps22_const::L * lla(3))); + BLA::Matrix<3, 1> dh_decef = {cosd(lla(0)) * cosd(lla(1)), cosd(lla(0)) * sind(lla(1)), sind(lla(0))}; + BLA::Matrix<3, 1> dP_decef = dP_dh * dh_decef; -// BLA::Matrix<20, 1> StateEstimator::EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R) { -// BLA::Matrix<3, 1> residual; -// residual = sens_reading - h; + BLA::Matrix<1, 19> H_baro; + H_baro.Fill(0); + H_baro.Submatrix<1, 3>(0, QMEKFInds::p_x - 1) = ~dP_decef; + H_baro(0, 18) = 1.0f; -// BLA::Matrix<3, 3> S; -// BLA::Matrix<19, 3> K; -// BLA::Matrix<19, 3> H_t = ~H; + BLA::Matrix<1, 1> R = lps22_const::baro_var; + + lastTimes(5) = curr_time; + return ekfCalcErrorInject(baro, H_baro, h_baro, R); +} + +template +BLA::Matrix<20, 1> StateEstimator::ekfCalcErrorInject(BLA::Matrix &sens_reading, BLA::Matrix H, BLA::Matrix h, BLA::Matrix R) { + BLA::Matrix residual = sens_reading - h; + + BLA::Matrix S; + BLA::Matrix<19, rows> K; + BLA::Matrix<19, rows> H_t = ~H; + + S = H * P * H_t + R; + K = (P * H_t) * BLA::Inverse(S); + BLA::Matrix<19, 1> postErrorState = K * residual; + + // Inject error angles into quat + BLA::Matrix<3, 1> alpha = extractSub(postErrorState, QMEKFInds::smallAngle); + + BLA::Matrix old_q = extractSub(x, QMEKFInds::quat); + BLA::Matrix<4, 1> q = quatMultiply(old_q, smallAngleRotVec2Quat(alpha)); + + x(0) = q(0); + x(1) = q(1); + x(2) = q(2); + x(3) = q(3); + x(4) += postErrorState(3); + x(5) += postErrorState(4); + x(6) += postErrorState(5); + x(7) += postErrorState(6); + x(8) += postErrorState(7); + x(9) += postErrorState(8); + x(10) += postErrorState(9); + x(11) += postErrorState(10); + x(12) += postErrorState(11); + x(13) += postErrorState(12); + x(14) += postErrorState(13); + x(15) += postErrorState(14); + x(16) += postErrorState(15); + x(17) += postErrorState(16); + x(18) += postErrorState(17); + x(19) += postErrorState(18); -// S = H * oldP * H_t + R; -// K = (oldP * H_t) * BLA::Inverse(S); -// BLA::Matrix<19, 1> postErrorState = K * residual; - -// // Inject error angles into quat -// BLA::Matrix<3, 1> alpha; -// alpha = {postErrorState(0), postErrorState(1), postErrorState(2)}; -// BLA::Matrix<3, 1> rotVec = 1.0f * alpha; -// float rotVecNorm = BLA::Norm(rotVec); -// BLA::Matrix<3,1> axis = rotVec / rotVecNorm; -// BLA::Matrix<4,1> dq = -// { -// cos(rotVecNorm/2.0f), -// axis(0) * sinf(rotVecNorm/2.0f), -// axis(1) * sinf(rotVecNorm/2.0f), -// axis(2) * sinf(rotVecNorm/2.0f), -// }; -// BLA::Matrix<4, 1> old_q = -// { -// x(0), -// x(1), -// x(2), -// x(3) -// }; -// BLA::Matrix<4, 1> q = quatMultiply(old_q, dq); - -// // Set quats -// x(0) = q(0); -// x(1) = q(1); -// x(2) = q(2); -// x(3) = q(3); - -// // Set velocity -// x(4) = x(4) + postErrorState(3); -// x(5) = x(5) + postErrorState(4); -// x(6) = x(6) + postErrorState(5); - -// // Set position -// x(7) = x(7) + postErrorState(6); -// x(8) = x(8) + postErrorState(7); -// x(9) = x(9) + postErrorState(8); - -// // Set gyro bias -// x(10) = x(10) + postErrorState(9); -// x(11) = x(11) + postErrorState(10); -// x(12) = x(12) + postErrorState(11); - -// // Set accel bias -// x(13) = x(13) + postErrorState(12); -// x(14) = x(14) + postErrorState(13); -// x(15) = x(15) + postErrorState(14); - -// // Set mag bias -// x(16) = x(16) + postErrorState(15); -// x(17) = x(17) + postErrorState(16); -// x(18) = x(18) + postErrorState(17); - -// // Set baro bias -// x(19) = x(19) + postErrorState(18); - -// return x; -// } + return x; +} diff --git a/src/qmekf.h b/src/qmekf.h index c0d0c76..b29d3d6 100644 --- a/src/qmekf.h +++ b/src/qmekf.h @@ -11,56 +11,45 @@ * @brief Struct holding the indices of the total QMEKF */ namespace QMEKFInds { -constexpr uint8_t q_w = 0; -constexpr uint8_t q_x = 1; -constexpr uint8_t q_y = 2; -constexpr uint8_t q_z = 3; -constexpr std::array quat = {q_w, q_x, q_y, q_z}; +constexpr int q_w = 0; +constexpr int q_x = 1; +constexpr int q_y = 2; +constexpr int q_z = 3; +BLA::Matrix<4, 1> quat = {q_w, q_x, q_y, q_z}; +BLA::Matrix<3, 1> smallAngle = {q_w, q_x, q_y}; -constexpr uint8_t v_x = 4; -constexpr uint8_t v_y = 5; -constexpr uint8_t v_z = 6; -constexpr std::array vel = {v_x, v_y, v_z}; +constexpr int v_x = 4; +constexpr int v_y = 5; +constexpr int v_z = 6; +BLA::Matrix<3, 1> vel = {v_x, v_y, v_z}; -constexpr uint8_t p_x = 7; -constexpr uint8_t p_y = 8; -constexpr uint8_t p_z = 9; -constexpr std::array pos = {p_x, p_y, p_z}; +constexpr int p_x = 7; +constexpr int p_y = 8; +constexpr int p_z = 9; +BLA::Matrix<3, 1> pos = {p_x, p_y, p_z}; -constexpr uint8_t gb_x = 10; -constexpr uint8_t gb_y = 11; -constexpr uint8_t gb_z = 12; -constexpr std::array gyroBias = {gb_x, gb_y, gb_z}; +constexpr int gb_x = 10; +constexpr int gb_y = 11; +constexpr int gb_z = 12; +BLA::Matrix<3, 1> gyroBias = {gb_x, gb_y, gb_z}; -constexpr uint8_t ab_x = 13; -constexpr uint8_t ab_y = 14; -constexpr uint8_t ab_z = 15; -constexpr std::array accelBias = {ab_x, ab_y, ab_z}; +constexpr int ab_x = 13; +constexpr int ab_y = 14; +constexpr int ab_z = 15; +BLA::Matrix<3, 1> accelBias = {ab_x, ab_y, ab_z}; -constexpr uint8_t mb_x = 16; -constexpr uint8_t mb_y = 17; -constexpr uint8_t mb_z = 18; -constexpr std::array magBias = {mb_x, mb_y, mb_z}; +constexpr int mb_x = 16; +constexpr int mb_y = 17; +constexpr int mb_z = 18; +BLA::Matrix<3, 1> magBias = {mb_x, mb_y, mb_z}; -constexpr uint8_t bb_z = 19; -constexpr std::array baroBias = {bb_z}; +constexpr int bb_z = 19; +BLA::Matrix<1, 1> baroBias = {bb_z}; }; // namespace QMEKFInds -#define QMEKF_LOG_DESC(X) \ - X(0, "w", p.print(state(QMEKFInds::q_w), 4)) \ - X(1, "i", p.print(state(QMEKFInds::q_x), 4)) \ - X(2, "j", p.print(state(QMEKFInds::q_y), 4)) \ - X(3, "k", p.print(state(QMEKFInds::q_z), 4)) \ - X(4, "v_x", p.print(state(QMEKFInds::v_x), 4)) \ - X(5, "v_y", p.print(state(QMEKFInds::v_y), 4)) \ - X(6, "v_z", p.print(state(QMEKFInds::v_z), 4)) \ - X(7, "p_x", p.print(state(QMEKFInds::p_x), 4)) \ - X(8, "p_y", p.print(state(QMEKFInds::p_y), 4)) \ - X(9, "p_z", p.print(state(QMEKFInds::p_z), 4)) \ - /** * @name QMEKFStateEstimator @@ -94,8 +83,11 @@ class StateEstimator { // Update Functions BLA::Matrix<20, 1> runAccelUpdate(BLA::Matrix<3, 1> a_b, float curr_time); BLA::Matrix<20, 1> runMagUpdate(BLA::Matrix<3, 1> m_b, float curr_time); - BLA::Matrix<20, 1> runGPSUpdate(BLA::Matrix<3, 1> gpsPos, BLA::Matrix<3, 1> gpsVel); - BLA::Matrix<20, 1> runBaroUpdate(BLA::Matrix<1, 1> baro); + BLA::Matrix<20, 1> runGPSUpdate(BLA::Matrix<3, 1> gpsPos, BLA::Matrix<3, 1> gpsVel, bool velOrientation, float curr_time); + BLA::Matrix<20, 1> runBaroUpdate(BLA::Matrix<1, 1> baro, float curr_time); + + template + BLA::Matrix<20, 1> ekfCalcErrorInject(BLA::Matrix &sens_reading, BLA::Matrix H, BLA::Matrix h, BLA::Matrix R); private: // Identity Matrices @@ -106,13 +98,12 @@ class StateEstimator { BLA::Matrix<3, 1> launch_ecef = {1475354.0f, -4490428.0f, 4268181.0f}; BLA::Matrix<3, 3> launch_dcmned2ecef; - // TODO this one is more confusing because can have diff sizes matrices - BLA::Matrix<20, 1> EKFCalcErrorInject(BLA::Matrix<20, 1> &oldState, BLA::Matrix<19, 19> &oldP, BLA::Matrix<3, 1> &sens_reading, BLA::Matrix<3, 19> H, BLA::Matrix<3, 1> h, BLA::Matrix<3, 3> R); - BLA::Matrix<6, 1> lastTimes = {0, 0, 0, 0, 0, 0}; // Gyro prop, accel prop, accel update, mag update, gps update, baro update BLA::Matrix<3,1> gyro_prev = {0, 0, 0}; - BLA::Matrix<3,1> accel_prev = {0, 0, 0}; + BLA::Matrix<3,1> accel_prev = {0, 0, 0}; // TODO wouldn't it be normal force + BLA::Matrix<3,1> vel_prev = {0, 0, 0}; + BLA::Matrix<3,1> pos_prev = launch_ecef; BLA::Matrix<3,1> mag_prev = {0, 0, 0}; BLA::Matrix<3,1> gps_pos_prev = {0, 0, 0}; BLA::Matrix<3,1> gps_vel_prev = {0, 0, 0}; From 6f5a9da1e7a10f468f17290158306530985f25c2 Mon Sep 17 00:00:00 2001 From: Abhay Mathur <66543532+abhayma1000@users.noreply.github.com> Date: Tue, 23 Dec 2025 11:40:46 -0500 Subject: [PATCH 10/14] Current prog from B term. Going on vacay --- platformio.ini | 24 +++-- src/QuaternionUtils.cpp | 224 +++++++++++++++++++++++++++++++--------- src/QuaternionUtils.h | 69 +++++++++++-- src/kfConsts.h | 47 +++++++++ src/main.cpp | 137 ++++++++++++++---------- src/qmekf.cpp | 61 ++++++++--- src/qmekf.h | 13 ++- 7 files changed, 443 insertions(+), 132 deletions(-) diff --git a/platformio.ini b/platformio.ini index 737b3c5..7f36563 100644 --- a/platformio.ini +++ b/platformio.ini @@ -8,13 +8,25 @@ ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html -[env:nucleo_h753zi] -platform = ststm32 -board = nucleo_h753zi +;[env:nucleo_h753zi] +;platform = ststm32 +;board = nucleo_h753zi +;framework = arduino +;lib_deps = +; tomstewart89/BasicLinearAlgebra @ ^5.1 +; Wire +; SPI +; Servo +; IWatchdog + + +[env:polaris] +board = teensymm +platform = teensy framework = arduino -lib_deps = +build_flags = -D POLARIS -D ENABLE_DEDICATED_SPI=0 +lib_deps = tomstewart89/BasicLinearAlgebra @ ^5.1 Wire SPI - Servo - IWatchdog + Servo \ No newline at end of file diff --git a/src/QuaternionUtils.cpp b/src/QuaternionUtils.cpp index 1b44b57..3966123 100644 --- a/src/QuaternionUtils.cpp +++ b/src/QuaternionUtils.cpp @@ -43,7 +43,7 @@ BLA::Matrix<3,1> QuaternionUtils::getRightVector(const BLA::Matrix<3,3> &rot) { } - +// Works BLA::Matrix<3, 3> QuaternionUtils::quat2DCM(const BLA::Matrix<4, 1> &quat) { float w = quat(0); float x = quat(1); @@ -70,7 +70,9 @@ BLA::Matrix<3, 3> QuaternionUtils::quat2DCM(const BLA::Matrix<4, 1> &quat) { return rot; } +// Works BLA::Matrix<3, 3> QuaternionUtils::skewSymmetric(const BLA::Matrix<3, 1> &vec) { + // Can rewrite as hstack BLA::Matrix<3, 3> mat; mat(0, 0) = 0; @@ -89,21 +91,24 @@ BLA::Matrix<3, 3> QuaternionUtils::skewSymmetric(const BLA::Matrix<3, 1> &vec) { } -BLA::Matrix<4, 1> QuaternionUtils::rotVec2Quat(const BLA::Matrix<3, 1> &vec) { +// Works +BLA::Matrix<4, 1> QuaternionUtils::rotVec2dQuat(const BLA::Matrix<3, 1> &vec) { BLA::Matrix<4, 1> quat; float norm = BLA::Norm(vec); BLA::Matrix<3, 1> vec_rot_normed = vec / norm; + float norm_over_two = norm / 2.0f; - quat(0, 0) = cos(norm / 2); - quat(1, 0) = vec_rot_normed(0, 0) * sin(norm / 2); - quat(2, 0) = vec_rot_normed(1, 0) * sin(norm / 2); - quat(3, 0) = vec_rot_normed(2, 0) * sin(norm / 2); + quat(0, 0) = cos(norm_over_two); + quat(1, 0) = vec_rot_normed(0, 0) * sin(norm_over_two); + quat(2, 0) = vec_rot_normed(1, 0) * sin(norm_over_two); + quat(3, 0) = vec_rot_normed(2, 0) * sin(norm_over_two); return quat; } -BLA::Matrix<4, 1> QuaternionUtils::smallAngleRotVec2Quat(const BLA::Matrix<3, 1> &vec) { +// Works +BLA::Matrix<4, 1> QuaternionUtils::smallAnglerotVec2dQuat(const BLA::Matrix<3, 1> &vec) { BLA::Matrix<4, 1> quat; quat(0, 0) = 1.0; quat(1, 0) = 0.5 * vec(0, 0); @@ -113,12 +118,48 @@ BLA::Matrix<4, 1> QuaternionUtils::smallAngleRotVec2Quat(const BLA::Matrix<3, 1> return quat; } +// Works BLA::Matrix<4, 1> QuaternionUtils::dcm2quat(const BLA::Matrix<3, 3> &dcm) { - // TODO - BLA::Matrix<4, 1> tmp_q = {1, 0, 0, 0}; - return tmp_q; + BLA::Matrix<4, 1> q_sq; + q_sq(0, 0) = 0.25f * (1.0f + dcm(0, 0) - dcm(1, 1) - dcm(2, 2)); + q_sq(1, 0) = 0.25f * (1.0f - dcm(0, 0) + dcm(1, 1) - dcm(2, 2)); + q_sq(2, 0) = 0.25f * (1.0f - dcm(0, 0) - dcm(1, 1) + dcm(2, 2)); + q_sq(3, 0) = 0.25f * (1.0f + dcm(0, 0) + dcm(1, 1) + dcm(2, 2)); + + int max_ind = vecMaxInd(q_sq); + BLA::Matrix<4, 1> q_ret; + + switch (max_ind) { + case 0: + q_ret(1, 0) = 4.0 * q_sq(0, 0); + q_ret(2, 0) = dcm(0, 1) + dcm(1, 0); + q_ret(3, 0) = dcm(2, 0) + dcm(0, 2); + q_ret(0, 0) = dcm(1, 2) - dcm(2, 1); + q_ret = q_ret / (4.0f * sqrt(q_sq(0, 0))); + case 1: + q_ret(1, 0) = dcm(0, 1) + dcm(1, 0); + q_ret(2, 0) = 4.0 * q_sq(1, 0); + q_ret(3, 0) = dcm(1, 2) + dcm(2, 1); + q_ret(0, 0) = dcm(2, 0) - dcm(0, 2); + q_ret = q_ret / (4.0f * sqrt(q_sq(1, 0))); + case 2: + q_ret(1, 0) = dcm(2, 0) + dcm(0, 2); + q_ret(2, 0) = dcm(1, 2) + dcm(2, 1); + q_ret(3, 0) = 4.0 * q_sq(2, 0); + q_ret(0, 0) = dcm(0, 1) - dcm(1, 0); + q_ret = q_ret / (4.0f * sqrt(q_sq(2, 0))); + case 3: + q_ret(1, 0) = dcm(1, 2) - dcm(2, 1); + q_ret(2, 0) = dcm(2, 0) - dcm(0, 2); + q_ret(3, 0) = dcm(0, 1) - dcm(1, 0); + q_ret(0, 0) = 4.0 * q_sq(3, 0); + q_ret = q_ret / (4.0f * sqrt(q_sq(3, 0))); + } + + return q_ret; } +// TODO test BLA::Matrix<4, 1> QuaternionUtils::quatMultiply(const BLA::Matrix<4, 1> &p, const BLA::Matrix<4, 1> &q) { BLA::Matrix<4, 1> quat; @@ -127,11 +168,12 @@ BLA::Matrix<4, 1> QuaternionUtils::quatMultiply(const BLA::Matrix<4, 1> &p, cons quat(2, 0) = p(0, 0) * q(2, 0) - p(1, 0) * q(3, 0) + p(2, 0) * q(1, 0) + p(3, 0) * q(1, 0); quat(3, 0) = p(0, 0) * q(3, 0) + p(1, 0) * q(2, 0) - p(2, 0) * q(1, 0) + p(3, 0) * q(3, 0); - - return QuaternionUtils::normalizeQuaternion(quat); + // Should we always normalize here? + return quat; } +// Note: Not super high precision like I want, however unlikely to convert this way BLA::Matrix<3, 1> QuaternionUtils::lla2ecef(const BLA::Matrix<3, 1> &lla) { float pi = 3.141592; float lat_rad = lla(0) * pi / 180.0; @@ -155,23 +197,28 @@ BLA::Matrix<3, 1> QuaternionUtils::lla2ecef(const BLA::Matrix<3, 1> &lla) { } +// TODO test again BLA::Matrix<3, 3> QuaternionUtils::dcm_ned2ecef(const BLA::Matrix<3, 1> &lla) { - float pi = 3.141592653; // TODO declare this somewhere else but idk c++ - float lat_rads = lla(0) * (pi / 180.0); - float lon_rads = lla(1) * (pi / 180.0); BLA::Matrix<3, 3> R_ET = { - -1.0f * sin(lat_rads) * cos(lon_rads), -1.0f * sin(lon_rads), -1.0f * cos(lat_rads) * cos(lon_rads), - -1.0f * sin(lat_rads) * sin(lon_rads), cos(lon_rads), -1.0f * cos(lat_rads) * sin(lon_rads), - cos(lat_rads), 0, -1.0 * sin(lat_rads) + -1.0f * sind(lla(0)) * cosd(lla(1)), -1.0f * sind(lla(1)), -1.0f * cosd(lla(0)) * cosd(lla(1)), + -1.0f * sind(lla(0)) * sind(lla(1)), cosd(lla(1)), -1.0f * cosd(lla(0)) * sind(lla(1)), + cosd(lla(0)), 0, -1.0f * sind(lla(0)) }; return R_ET; } +// TODO one day test BLA::Matrix<3, 1> QuaternionUtils::ecef2nedVec(const BLA::Matrix<3, 1> &ecef_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET) { return ~R_ET * (ecef_meas - launch_ecef); } +// TODO test +BLA::Matrix<3, 1> QuaternionUtils::ned2ecefVec(const BLA::Matrix<3, 1> &ned_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET) { + return R_ET * ned_meas + launch_ecef; +} + +// Works BLA::Matrix<4, 1> QuaternionUtils::quatConjugate(const BLA::Matrix<4, 1> &p){ BLA::Matrix<4, 1> quat; quat(0, 0) = p(0); @@ -182,25 +229,34 @@ BLA::Matrix<4, 1> QuaternionUtils::quatConjugate(const BLA::Matrix<4, 1> &p){ return quat; } +// TODO test +BLA::Matrix<3, 1> QuaternionUtils::qRot(const BLA::Matrix<4, 1> &q, BLA::Matrix<3, 1> vec) { + return QuaternionUtils::quat2DCM(q) * vec; +} + +// TODO test when actually implement grav world model BLA::Matrix<3, 1> QuaternionUtils::g_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef) { - // TODO one day put in the world model + // TODO one day put in the world model and take in position BLA::Matrix<3, 1> grav_ned = {0, 0, 9.8}; return ~dcm_ned2ecef * grav_ned; } +// TODO test when actually implement mag world model BLA::Matrix<3, 1> QuaternionUtils::m_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef) { - // TODO one day put in the world model. School vals for now + // TODO one day put in the world model and take in position. School vals for now BLA::Matrix<3, 1> mag_ned = {19.983111, -4.8716, 46.9986145}; return ~dcm_ned2ecef * mag_ned; } +// TODO test when actually implement grav world model BLA::Matrix<3, 1> QuaternionUtils::normal_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef) { - // TODO one day put in the world model + // TODO one day put in the world model. See g_i notes BLA::Matrix<3, 1> normal_ned = {0, 0, -9.8}; return ~dcm_ned2ecef * normal_ned; } BLA::Matrix<3, 3> QuaternionUtils::vecs2mat(const BLA::Matrix<3, 1> v1, const BLA::Matrix<3, 1> v2, const BLA::Matrix<3, 1> v3) { + // TODO eventually replace with just using hstack BLA::Matrix<3, 3> ret = {v1(0, 0), v2(0), v3(0, 0), v1(1, 0), v2(1, 0), v3(1, 0), v1(2, 0), v2(2, 0), v3(2, 0)}; @@ -209,25 +265,25 @@ BLA::Matrix<3, 3> QuaternionUtils::vecs2mat(const BLA::Matrix<3, 1> v1, const BL } - +// TODO fix BLA::Matrix<4, 1> QuaternionUtils::triad_BE(const BLA::Matrix<3, 1> v1_b, const BLA::Matrix<3, 1> v2_b, const BLA::Matrix<3, 1> v1_i, const BLA::Matrix<3, 1> v2_i) { // 1 is important, 2 is secondary BLA::Matrix<3, 1> v1_b_norm = v1_b / BLA::Norm(v1_b); - BLA::Matrix<3, 1> v2_b_norm = v1_b / BLA::Norm(v2_b); - BLA::Matrix<3, 1> v1_i_norm = v1_b / BLA::Norm(v1_i); - BLA::Matrix<3, 1> v2_i_norm = v1_b / BLA::Norm(v2_i); + BLA::Matrix<3, 1> v2_b_norm = v2_b / BLA::Norm(v2_b); + BLA::Matrix<3, 1> v1_i_norm = v1_i / BLA::Norm(v1_i); + BLA::Matrix<3, 1> v2_i_norm = v2_b / BLA::Norm(v2_i); // Inertial - BLA::Matrix<3, 1> q_r = v1_i; - BLA::Matrix<3, 1> r_r = BLA::CrossProduct(v1_i, v2_i) / BLA::Norm(BLA::CrossProduct(v1_i, v2_i)); + BLA::Matrix<3, 1> q_r = v1_i_norm; + BLA::Matrix<3, 1> r_r = BLA::CrossProduct(v1_i_norm, v2_i_norm) / BLA::Norm(BLA::CrossProduct(v1_i_norm, v2_i_norm)); BLA::Matrix<3, 1> s_r = BLA::CrossProduct(q_r, r_r); BLA::Matrix<3, 3> M_r = vecs2mat(q_r, r_r, s_r); // Body - BLA::Matrix<3, 1> q_b = v1_b; - BLA::Matrix<3, 1> r_b = BLA::CrossProduct(v1_b, v2_b) / BLA::Norm(BLA::CrossProduct(v1_b, v2_b)); + BLA::Matrix<3, 1> q_b = v1_b_norm; + BLA::Matrix<3, 1> r_b = BLA::CrossProduct(v1_b_norm, v2_b_norm) / BLA::Norm(BLA::CrossProduct(v1_b_norm, v2_b_norm)); BLA::Matrix<3, 1> s_b = BLA::CrossProduct(q_b, r_b); BLA::Matrix<3, 3> M_b = vecs2mat(q_b, r_b, s_b); @@ -238,7 +294,15 @@ BLA::Matrix<4, 1> QuaternionUtils::triad_BE(const BLA::Matrix<3, 1> v1_b, const } -BLA::Matrix<3,1> QuaternionUtils::lla2ecef(BLA::Matrix<3,1> lla) { +BLA::Matrix<4, 1> QuaternionUtils::quest_BE(const BLA::Matrix<3, 100> local_vecs, const BLA::Matrix<3, 100> ref_vecs, BLA::Matrix weights) { + // Note: IF we have multiple vec measurements, there are tradeoffs between: + // Certifiable optimality, speed (microprocessor wise), determinism, and robustness to outliers + // Here I implement quest for 3+ vector meas, but read this and do research if other alg is desired: + // https://pdfs.semanticscholar.org/d30c/b21689c63660ca1de4c9c52ef3ae5b92e19b.pdf (chinese ppl unfortunately) +} + +// TODO test +BLA::Matrix<3,1> QuaternionUtils::lla2ecef2(BLA::Matrix<3,1> lla) { float pi = 3.141592653589; // TODO move this somewhere better (idk c++) float a = 6378137.0; // WGS-84 semi-major axis float f = 1.0 / 298.257223563; // flattening @@ -261,16 +325,17 @@ BLA::Matrix<3,1> QuaternionUtils::lla2ecef(BLA::Matrix<3,1> lla) { } +// TODO test BLA::Matrix<3, 1> QuaternionUtils::ecef2lla(BLA::Matrix<3, 1> ecef) { - // Follows this kid idk if right, gotta test, but I'm not writing this bs myself - // I think this is's impl not super precise, but not that deep (I think) + // Follows this kid idk if right, gotta test, but I'm not writing this bs from scratch + // I think this is's impl not super precise, but not that deep (I think). LLA is stupid // https://github.com/kvenkman/ecef2lla/blob/master/ecef2lla.py float pi = 3.141592653589; float a = 6378137.0f; float a_sq = a * a; - float e = 8.181919084261345e-2; - float e_sq = 6.69437999014e-3; + float e = 8.181919084261345e-2f; + float e_sq = 6.69437999014e-3f; float f = 1.0f/298.257223563; float b = a*(1.0-f); @@ -284,36 +349,93 @@ BLA::Matrix<3, 1> QuaternionUtils::ecef2lla(BLA::Matrix<3, 1> ecef) { float s = std::pow((1.0 + c + std::sqrt(c * c + 2 * c)), (1.0f / 3.0f)); float p = f / (3.0f * (g * g) * std::pow(s + (1.0 / s) + 1.0, 2.0f)); float q = std::sqrt(1.0f + 2.0f * p * std::pow(e_sq, 2.0f)); - // float r_0 = - // TODO got lazy, gotta finish this tho - // r_0 = -(p*e_sq*r)/(1+q) + np.sqrt(0.5*(a**2)*(1+(1./q)) - p*(z**2)*(1-e_sq)/(q*(1+q)) - 0.5*p*(r**2)) - // u = np.sqrt((r - e_sq*r_0)**2 + z**2) - // v = np.sqrt((r - e_sq*r_0)**2 + (1 - e_sq)*z**2) - // z_0 = (b**2)*z/(a*v) - // h = u*(1 - b**2/(a*v)) - // phi = np.arctan((z + ep_sq*z_0)/r) - // lambd = np.arctan2(y, x) - - float phi = 0.0f; - float lambd = 0.0f; - float h = 0.0f; + float r_0 = -1.0f * (p * e_sq * r) / (1.0 + q) + std::sqrt(0.5f * std::pow(a, 2) * (1.0f + (1.0f / q)) - 0.5f * p * std::pow(r, 2)); + + float u = std::sqrt(std::pow(r - e_sq * r_0, 2) + std::pow(ecef(2), 2)); + float v = std::sqrt(std::pow(r - e_sq * r_0, 2) + (1.0f - e_sq) * std::pow(ecef(2), 2)); + float z_0 = std::pow(b, 2) * ecef(2) / (a * v); + float h = u * (1.0f - std::pow(b, 2) / (a * v)); + float phi = std::atan((ecef(2) + ep_sq * z_0) / r); + float lambd = std::atan2(ecef(1), ecef(0)); + BLA::Matrix<3, 1> lla = {phi*180.0f/pi, lambd*180.0/pi, h}; return lla; } +// Do this if above is inaccurate +/* +def ecef_to_lla(ecef): + '''Convert ECEF (meters) coordinates to geodetic coordinates (degrees and meters). + It would be good to check out the OG source and make sure this is appropriate. + Uses those parameters right above here. + + Sourced from https://possiblywrong.wordpress.com/2014/02/14/when-approximate-is-better-than-exact/ + Originally from + Olson, D. K., Converting Earth-Centered, Earth-Fixed Coordinates to + Geodetic Coordinates, IEEE Transactions on Aerospace and Electronic + Systems, 32 (1996) 473-476. + + Parameters + ---------- + ecef : numpy.ndarray + Position of satellite in ECEF frame. + + Returns + ------- + list + Latitude, longitude, and vertical distance above geode along surface normal. + ''' + w = math.sqrt(ecef[0] * ecef[0] + ecef[1] * ecef[1]) + z = ecef[2] + zp = abs(z) + w2 = w * w + r2 = z * z + w2 + r = math.sqrt(r2) + s2 = z * z / r2 + c2 = w2 / r2 + u = a2 / r + v = a3 - a4 / r + if c2 > 0.3: + s = (zp / r) * (1 + c2 * (a1 + u + s2 * v) / r) + lat = math.asin(s) + ss = s * s + c = math.sqrt(1 - ss) + else: + c = (w / r) * (1 - s2 * (a5 - u - c2 * v) / r) + lat = math.acos(c) + ss = 1 - c * c + s = math.sqrt(ss) + g = 1 - e2 * ss + rg = a / math.sqrt(g) + rf = a6 * rg + u = w - rg * c + v = zp - rf * s + f = c * u + s * v + m = c * v - s * u + p = m / (rf / g + f) + lat = lat + p + if z < 0: + lat = -lat + return (np.degrees(lat), np.degrees(math.atan2(ecef[1], ecef[0])), f + m * p / 2) +*/ + +// TODO test BLA::Matrix<4, 1> QuaternionUtils::normalizeQuaternion(BLA::Matrix<4, 1> quat) { BLA::Matrix<4, 1> new_quat = quat / BLA::Norm(quat); return new_quat; } -float QuaternionUtils::cosd(float rads) { +float QuaternionUtils::cosd(float degs) { float pi = 3.141592653589; - return cos(rads * (180.0f / pi)); + return cos(degs * (pi / 180.0f)); } -float QuaternionUtils::sind(float rads) { +float QuaternionUtils::sind(float degs) { float pi = 3.141592653589; - return sin(rads * (180.0f / pi)); + return sin(degs * (pi / 180.0f)); } + + + diff --git a/src/QuaternionUtils.h b/src/QuaternionUtils.h index ab10061..d24860a 100644 --- a/src/QuaternionUtils.h +++ b/src/QuaternionUtils.h @@ -27,10 +27,10 @@ namespace QuaternionUtils { BLA::Matrix<3, 3> skewSymmetric(const BLA::Matrix<3, 1> &vec); // Rotation vector to quaternion - BLA::Matrix<4, 1> rotVec2Quat(const BLA::Matrix<3, 1> &vec); + BLA::Matrix<4, 1> rotVec2dQuat(const BLA::Matrix<3, 1> &vec); // Small angle rot vec to quat - BLA::Matrix<4, 1> smallAngleRotVec2Quat(const BLA::Matrix<3, 1> &vec); + BLA::Matrix<4, 1> smallAnglerotVec2dQuat(const BLA::Matrix<3, 1> &vec); BLA::Matrix<4, 1> dcm2quat(const BLA::Matrix<3, 3> &dcm); @@ -43,9 +43,13 @@ namespace QuaternionUtils { BLA::Matrix<3, 1> ecef2nedVec(const BLA::Matrix<3, 1> &ecef_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET); + BLA::Matrix<3, 1> ned2ecefVec(const BLA::Matrix<3, 1> &ned_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET); + //Quaternion Conjugate BLA::Matrix<4, 1> quatConjugate(const BLA::Matrix<4, 1> &p); + BLA::Matrix<3, 1> qRot(const BLA::Matrix<4, 1> &q, BLA::Matrix<3, 1> vec); + BLA::Matrix<3, 1> g_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef); BLA::Matrix<3, 1> m_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef); @@ -56,7 +60,7 @@ namespace QuaternionUtils { BLA::Matrix<4, 1> triad_BE(const BLA::Matrix<3, 1> v1_b, const BLA::Matrix<3, 1> v2_b, const BLA::Matrix<3, 1> v1_i, const BLA::Matrix<3, 1> v2_i); - BLA::Matrix<3, 1> lla2ecef(BLA::Matrix<3,1> lla); + BLA::Matrix<3, 1> lla2ecef2(BLA::Matrix<3,1> lla); BLA::Matrix<3, 1> ecef2lla(BLA::Matrix<3, 1> ecef); @@ -99,6 +103,19 @@ namespace QuaternionUtils { return maxVal; } + template + int vecMaxInd(const BLA::Matrix &x) { + float maxVal = x(0, 0); + int maxInd = 0; + for (int i = 0; i < N; i++) { + if (x(i, 0) > maxVal) { + maxVal = x(i, 0); + maxInd = i; + } + } + return maxInd; + } + template BLA::Matrix setSub(BLA::Matrix &x, const BLA::Matrix inds, const BLA::Matrix vals) { for (int i = 0; i < M; i++) { @@ -109,11 +126,11 @@ namespace QuaternionUtils { BLA::Matrix<4, 1> normalizeQuaternion(BLA::Matrix<4, 1> quat); - float cosd(float rads); - float sind(float rads); + float cosd(float degs); + float sind(float degs); template - BLA::Matrix stack(BLA::Matrix x, BLA::Matrix y) { + BLA::Matrix vstack(BLA::Matrix x, BLA::Matrix y) { BLA::Matrix ret; for (int i = 0; i < N; i++) { ret(i, 0) = x(i, 0); @@ -123,4 +140,44 @@ namespace QuaternionUtils { } return ret; } + + template + BLA::Matrix hstack(BLA::Matrix first, BLA::Matrix second) { + BLA::Matrix ret; + + for (int i = 0; i < M; i++) { + for (int j = 0; j < N; j++) { + ret(i, j) = first(i, j); + } + for (int k = 0; k < N2; k++) { + ret(i, N+k) = second(i, k); + } + } + + return ret; + } + + template + float sum(BLA::Matrix x) { + float ret = 0.0; + + for (int i = 0; i < N; i++) { + ret += x(i, 0); + } + return ret; + } + + template + void printMatHighDef(BLA::Matrix x) { + Serial.println("{"); + for (int i = 0; i < M; i++) { + Serial.print("{"); + for (int j = 0; j < N; j++) { + Serial.print(x(i, j), 6); + Serial.print(", "); + } + Serial.println("}"); + } + Serial.println("}\n"); + } } diff --git a/src/kfConsts.h b/src/kfConsts.h index c7668b8..c34dd9a 100644 --- a/src/kfConsts.h +++ b/src/kfConsts.h @@ -2,10 +2,20 @@ #include "BasicLinearAlgebra.h" #include +#include "QuaternionUtils.h" + +using namespace QuaternionUtils; + constexpr static float g = 9.80665; // [m/s/s] Earth's Grav Accel TODO eventually take out when have wgs model // TODO: Fill out this file with correct terms and our units constexpr float square(float a) { return a * a; } +namespace mars_const { + // BLA::Matrix<3, 3> R_BM = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + // BLA::Matrix<3, 1> p_MB_B = {-0.5, 0.1, } + // No, TODO: Do as dual quaternions +} + namespace asm330_const { constexpr float accelXY_var = square(0.0020f); // [g] constexpr float accelZ_var = square(0.0014f); // [g] @@ -19,6 +29,12 @@ constexpr float gyro_VRW = 0.0241f; // [deg/s/sqrt(Hz)] BLA::Matrix<3, 1> accel_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; // TODO these are in g's, mode to m/s^2 BLA::Matrix<3, 1> gyro_var = {square(0.0947f), square(0.0947f), square(0.0947f)}; // TODO these are in deg, mode to rads + +BLA::Matrix<1, 1> avg_accel_var = {sum(accel_var) / 3.0f}; +BLA::Matrix<1, 1> avg_gyro_var = {sum(gyro_var) / 3.0f}; + +BLA::Matrix<3, 1> loc = {-0.1, 0.05, 0.05}; + }; // namespace asm330_const namespace lps22_const { @@ -50,6 +66,11 @@ BLA::Matrix<3, 1> gyro_var = {square(0.0947f), square(0.0947f), square(0.0947f)} BLA::Matrix<3, 1> mag_var = {1.0f, 2.0f, 3.0f}; // [uT] BLA::Matrix<3, 1> mag_bias = {0, 0, 0}; + +BLA::Matrix<1, 1> avg_accel_var = {sum(accel_var) / 3.0f}; +BLA::Matrix<1, 1> avg_gyro_var = {sum(gyro_var) / 3.0f}; + +BLA::Matrix<3, 1> loc = {-0.1, 0.05, 0.05}; }; // namespace icm20948_const @@ -60,8 +81,34 @@ namespace Max10S_const { namespace vimu_const { // TODO vimu/2imu + BLA::Matrix<2, 1> ones = {1, 1}; // TODO implement algorithm here (or don't and just do it in matlab script). For right now, just using ICM values + + // TODO for this implement getting the inverse (1 / ) for each entry + BLA::Matrix<2, 2> avg_accel_vars_inv = toDiag(vstack(icm20948_const::avg_accel_var, asm330_const::avg_accel_var)); + + BLA::Matrix<3, 2> R = hstack(icm20948_const::loc, asm330_const::loc); + + BLA::Matrix<3, 1> r_bar = R * avg_accel_vars_inv * ones; + + BLA::Matrix<3, 3> M = R * avg_accel_vars_inv * ~R; + + // BLA::Matrix<2, 1> w_hat = avg_accel_vars_inv * (ones - ~R * (M_pinv * r_bar)); + + // BLA::Matrix<2, 1> w_accel = w_hat / sum(w_hat); + + // TODO combine accel vars + + + BLA::Matrix<2, 2> avg_gyro_vars_inv = BLA::Inverse(toDiag(vstack(icm20948_const::avg_gyro_var, asm330_const::avg_gyro_var))); + // TODO do it correctly + + // BLA::Matrix<2, 1> w_gyro = avg_gyro_vars_inv / + + + + BLA::Matrix<3, 1> accel_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; // TODO these are in g's, mode to m/s^2 BLA::Matrix<3, 1> gyro_var = {square(0.0947f), square(0.0947f), square(0.0947f)}; // TODO these are in deg, mode to rads diff --git a/src/main.cpp b/src/main.cpp index f029215..d74968e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,95 +4,124 @@ #include "QuaternionUtils.h" +using namespace QuaternionUtils; +// This line is a game changer + void setup() { -// // Since basing this whole c++ impl on the simulink, using that as reference here -// Serial.println("Hello"); -// BLA::Matrix<4, 1> random_quat = {0.9344, 0.2435, 0.2435, 0.0907}; -// BLA::Matrix<4, 1> random_quat2 = {0.3666, 0.4564, 0.7857, 0.1998}; -// BLA::Matrix<3, 3> random_mat = {0.8662, 0.0206,0.2123, 0.6011, 0.9699, 0.1818, 0.7081, 0.8324, 0.1834}; -// BLA::Matrix<3, 1> random_vec = {0.3042, 0.5248, 0.4319}; -// BLA::Matrix<3, 1> school_lla = {42.274027, -71.811788, 10}; -// Serial.println("Test quat2dcm: Quat: {0.9344, 0.2435, 0.2435, 0.0907}. Expected DCM: \ -// 0.8650 0.2880 -0.4109 \ -// -0.0508 0.8649 0.4993 \ -// 0.4992 -0.4110 0.7628. Actual DCM:"); +} + +void loop() { + BLA::Matrix<4, 1> random_quat = {0.9344, 0.2435, 0.2435, 0.0907}; + BLA::Matrix<3, 3> assoc_dcm = {0.8650, 0.2880, -0.4109, -0.0508, 0.8649, 0.4993, 0.4992, -0.4110, 0.7628}; + BLA::Matrix<4, 1> random_quat2 = {0.3666, 0.4564, 0.7857, 0.1998}; + BLA::Matrix<3, 3> random_mat = {0.8662, 0.0206,0.2123, 0.6011, 0.9699, 0.1818, 0.7081, 0.8324, 0.1834}; + BLA::Matrix<3, 1> random_vec = {0.3042, 0.5248, 0.4319}; + BLA::Matrix<3, 1> school_lla = {42.274027, -71.811788, 10}; + BLA::Matrix<3, 1> test_launch_ecef = {1, 2, 3}; + BLA::Matrix<3, 1> test_end_ecef = {1, 2, 3}; + BLA::Matrix<3, 1> v1_b = {-8.4870, -4.7330, -1.2682}; + BLA::Matrix<3, 1> v2_b = {-0.1710, 0.0429, 0.9843}; + BLA::Matrix<3, 1> v1_i = {0, 0, 9.8}; + BLA::Matrix<3, 1> v2_i = {1, 0, 0}; + BLA::Matrix<4, 1> random_unnormed_quat = {21.232, 40.243, 50.233, 19.232}; + +// Serial.println("Test quat2dcm: Quat: {0.9344, 0.2435, 0.2435, 0.0907}. Expected DCM: \ +// 0.8650 0.2880 -0.4109 \ +// -0.0508 0.8649 0.4993 \ +// 0.4992 -0.4110 0.7628. Actual DCM:"); -// Serial.println(QuaternionUtils::quat2DCM(random_quat)); +// printMatHighDef(quat2DCM(random_quat)); +// Serial.println("Test skewSymmetric: Random vec: {0.3042, 0.5248, 0.4319}. Expected Mat: \ +// 0 -0.4319 0.5248 \ +// 0.4319 0 -0.3042 \ +// -0.5248 0.3042 0"); -// Serial.println("Test skewSymmetric: Random vec: {0.3042, 0.5248, 0.4319}. Expected Mat: \ -// 0 -0.4319 0.5248 \ -// 0.4319 0 -0.3042 \ -// -0.5248 0.3042 0"); +// printMatHighDef(skewSymmetric(random_vec)); -// Serial.println(QuaternionUtils::skewSymmetric(random_vec)); +// Serial.println("Test rotVec2dQuat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {0.9315, 0.1486, 0.2564, 0.2110} "); +// printMatHighDef(rotVec2dQuat(random_vec)); -// Serial.println("Test rotVec2Quat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {0.9315, 0.2769, 0.4776, 0.3931}"); -// Serial.println(QuaternionUtils::rotVec2Quat(random_vec)); +// Serial.println("Test smallAnglerotVec2dQuat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {1.0000, 0.1521, 0.2624, 0.2160}"); +// printMatHighDef(smallAnglerotVec2dQuat(random_vec)); -// Serial.println("Test smallAngleRotVec2Quat: Random vec: {0.3042, 0.5248, 0.4319}. Expected Quat: {1.0000, 0.1521, 0.2624, 0.2160}"); +// Serial.println("Test dcm2quat: Dcm: 0.8650 0.2880 -0.4109 \ +// -0.0508 0.8649 0.4993 \ +// 0.4992 -0.4110 0.7628. Expected quat: {0.9344, 0.2435, 0.2435, 0.0907}"); -// Serial.println(QuaternionUtils::smallAngleRotVec2Quat(random_vec)); +// printMatHighDef(dcm2quat(assoc_dcm)); +// Serial.println("Test quatMultiply: Random Quats: {0.9344 0.2435 0.2435 0.0907} and {0.3666 0.4564 0.7857 0.1998} \ +// Expected Quat: {0.0220 0.4932 0.8162 0.3002}"); -// // TODO eventually test dcm2quat +// printMatHighDef(quatMultiply(random_quat, random_quat2)); -// Serial.println("Test quatMultiply: Random Quats: {0.9344 0.2435 0.2435 0.0907} and {0.3666 0.4564 0.7857 0.1998} \ -// Expected Quat: {0.0220 0.4932 0.8162 0.3002}"); +// Serial.println("Test lla2ecef: School LLA: {42.274027, -71.811788, 10}. Expected ECEF: {1475354, -4490428, 4268181}"); -// Serial.println(QuaternionUtils::quatMultiply(random_quat, random_quat2)); +// printMatHighDef(lla2ecef(school_lla)); +// Serial.println("Test lla2ecef2: School LLA: {42.274027, -71.811788, 10}. Expected ECEF: {1475354, -4490428, 4268181}"); -// Serial.println("Test lla2ecef: School LLA: {42.274027, -71.811788, 10}. Expected ECEF: {1475354, -4490428, 4268181}"); +// printMatHighDef(lla2ecef2(school_lla)); -// Serial.println(QuaternionUtils::lla2ecef(school_lla)); +// Serial.println("Test dcm_ned2ecef: School LLA: {42.274027, -71.811788, 10}. Expected DCM: \ +// -0.2100 0.9500 -0.2310 \ +// 0.6391 0.3121 0.7030 \ +// 0.7399 0 -0.6727"); +// printMatHighDef(dcm_ned2ecef(school_lla)); -// Serial.println("Test dcm_ned2ecef: School LLA: {42.274027, -71.811788, 10}. Expected DCM: \ -// -0.2100 0.9500 -0.2310 \ -// 0.6391 0.3121 0.7030 \ -// 0.7399 0 -0.6727"); -// Serial.println(QuaternionUtils::dcm_ned2ecef(school_lla)); +// // Serial.println("Test ecef2nedVec:"); // Eh one day -// Serial.println("Test ecef2nedVec:"); // Eh one day +// Serial.println("Test quatConj: Test quat: {0.9344 0.2435 0.2435 0.0907}. Expected quat: 0.9344 -0.2435 -0.2435 -0.0907"); +// printMatHighDef(quatConjugate(random_quat)); -// Serial.println("Test quatConj: Test quat: {0.9344 0.2435 0.2435 0.0907}. Expected quat: 0.9344 -0.2435 -0.2435 -0.0907"); -// Serial.println(QuaternionUtils::quatConjugate(random_quat)); -// BLA::Matrix<3, 1> v1_b = {-8.4870, -4.7330, -1.2682}; -// BLA::Matrix<3, 1> v2_b = {-0.1710, 0.0429, 0.9843}; -// BLA::Matrix<3, 1> v1_i = {0, 0, 9.8}; -// BLA::Matrix<3, 1> v2_i = {1, 0, 0}; + // Serial.println("Test triad_BE: Test vecs displayed here in code. Expected: \ + // -0.1710 -0.4698 -0.8660 \ + // 0.0429 0.8746 -0.4830 \ + // 0.9843 -0.1197 -0.1294"); + // printMatHighDef(triad_BE(v1_b, v2_b, v1_i, v2_i)); -// Serial.println("Test triad_BE: Test vecs displayed here in code. Expected: \ -// -0.1710 -0.4698 -0.8660 \ -// 0.0429 0.8746 -0.4830 \ -// 0.9843 -0.1197 -0.1294"); -// Serial.println(QuaternionUtils::triad_BE(v1_b, v2_b, v1_i, v2_i)); + // Serial.println("Test extractSub. Expected {42.274027, 10}"); + // BLA::Matrix<2, 1> inds = {0, 22}; + // printMatHighDef(extractSub(school_lla, inds)); - // Serial.println("TODO Test lla2ecef"); + // Serial.println("Test extractDiag. Expected: {0.8662, 0.9699, 0.1834}"); + // printMatHighDef(extractDiag(random_mat)); - // Serial.println("TODO test ecef2lla"); + Serial.println("Test g_i_ecef using school coords. Expected: TODO"); + printMatHighDef(g_i_ecef(dcm_ned2ecef(school_lla))); - // Serial.println("Test extractSub"); - // Serial.println(QuaternionUtils::extractSub(school_lla, {0, 2})); + Serial.println("Test m_i_ecef using school coords. Expected: TODO"); + printMatHighDef(m_i_ecef(dcm_ned2ecef(school_lla))); - // Serial.println("Test extractDiag"); - // Serial.println(QuaternionUtils::extractDiag(random_mat)); - + Serial.println("Test normal_i_ecef using school coords. Expected: TODO"); + printMatHighDef(normal_i_ecef(dcm_ned2ecef(school_lla))); -} + Serial.println("Test normalizeQuaterion. Initial quat: {21.232, 40.243, 50.233, 19.232}\n \ + Expected: {0.3014, 0.5712, 0.7130, 0.2730}"); + printMatHighDef(normalizeQuaternion(random_unnormed_quat)); -void loop() { -} \ No newline at end of file + Serial.println("Test cosd. Initial angle: -270. Expected: 0"); + Serial.println(cosd(-270)); + + Serial.println("Test sind. Initial angle: -270. Expected: 1"); + Serial.println(sind(-270)); + + + + Serial.println("\n\n\n\n\n"); + delay(10000); +} diff --git a/src/qmekf.cpp b/src/qmekf.cpp index 4a2eb8b..0837064 100644 --- a/src/qmekf.cpp +++ b/src/qmekf.cpp @@ -93,7 +93,7 @@ BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float cur // TODO combine gyro through vimu BLA::Matrix<3, 1> unbiased_gyro = gyro - extractSub(x, QMEKFInds::gyroBias); - BLA::Matrix<4, 1> new_q = quatMultiply(extractSub(x, QMEKFInds::quat), rotVec2Quat(unbiased_gyro)); + BLA::Matrix<4, 1> new_q = quatMultiply(extractSub(x, QMEKFInds::quat), rotVec2dQuat(unbiased_gyro * dt)); x = setSub(x, QMEKFInds::quat, new_q); gyro_prev = unbiased_gyro; lastTimes(0) = curr_time; @@ -101,13 +101,13 @@ BLA::Matrix<20, 1> StateEstimator::fastGyroProp(BLA::Matrix<3,1> gyro, float cur } BLA::Matrix<20, 1> StateEstimator::fastAccelProp(BLA::Matrix<3, 1> accel, float curr_time) { - BLA::Matrix<4, 1> last_relevant_times = (1, 4, 5); + BLA::Matrix<3, 1> last_relevant_times = {1, 4, 5}; float dt = curr_time - vecMax(extractSub(lastTimes, last_relevant_times)); // TODO combine accel through vimu BLA::Matrix<3, 1> unbiased_accel = accel - extractSub(x, QMEKFInds::accelBias); - BLA::Matrix<3, 3> q_conj_dcm = quat2DCM(quatConjugate((x, QMEKFInds::quat))); + BLA::Matrix<3, 3> q_conj_dcm = quat2DCM(quatConjugate(extractSub(x, QMEKFInds::quat))); BLA::Matrix<3, 1> v = ((q_conj_dcm * unbiased_accel + q_conj_dcm * accel_prev) / 2.0f) + g_i_ecef(launch_dcmned2ecef) * dt + vel_prev; BLA::Matrix<3, 1> p = ((v + vel_prev) / 2.0f) * dt + pos_prev; x = setSub(x, QMEKFInds::vel, v); @@ -122,16 +122,17 @@ BLA::Matrix<20, 1> StateEstimator::fastAccelProp(BLA::Matrix<3, 1> accel, float BLA::Matrix<19, 19> StateEstimator::ekfPredict(float curr_time) { + // TODO finish this. too lazy BLA::Matrix<4, 1> att_last_relevant_times = {0, 1, 3, 4}; - BLA::Matrix<4, 1> pv_last_relevant_times = (1, 4, 5); - float att_dt = curr_time - vecMax(extractSub(lastTimes, att_last_relevant_times)); + BLA::Matrix<4, 1> pv_last_relevant_times = {1, 4, 5}; + // float att_dt = curr_time - vecMax(extractSub(lastTimes, att_last_relevant_times)); float pv_dt = curr_time - vecMax(extractSub(lastTimes, pv_last_relevant_times)); BLA::Matrix<3,3> gyroSkew = skewSymmetric(gyro_prev); BLA::Matrix<3,3> accelSkew = skewSymmetric(accel_prev); - BLA::Matrix<3, 3> q_conj_dcm = quat2DCM(quatConjugate((x, QMEKFInds::quat))); + BLA::Matrix<3, 3> q_conj_dcm = quat2DCM(quatConjugate(extractSub(x, QMEKFInds::quat))); BLA::Matrix<19, 19> F; F.Fill(0); @@ -248,13 +249,14 @@ BLA::Matrix<20, 1> StateEstimator::runMagUpdate(BLA::Matrix<3, 1> m_b, float cur } BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<3, 1> gpsPos, BLA::Matrix<3, 1> gpsVel, bool velOrientation, float curr_time) { + // TODO something is wrong with gps vel lastTimes(4) = curr_time; if (velOrientation) { - BLA::Matrix<9, 1> combined_sens = stack(stack(gpsVel, gpsPos), gpsVel); + BLA::Matrix<9, 1> combined_sens = vstack(vstack(gpsVel, gpsPos), gpsVel); BLA::Matrix<3, 1> v_b = {BLA::Norm(gpsVel), 0, 0}; BLA::Matrix<4, 1> q = extractSub(x, QMEKFInds::quat); BLA::Matrix<3, 1> h_vi = quat2DCM(quatConjugate(q)) * v_b; - BLA::Matrix<9, 1> h_gps = stack(extractSub(x, stack(QMEKFInds::vel, QMEKFInds::pos)), extractSub(x, QMEKFInds::vel)); + BLA::Matrix<9, 1> h_gps = vstack(extractSub(x, vstack(QMEKFInds::vel, QMEKFInds::pos)), extractSub(x, QMEKFInds::vel)); BLA::Matrix<9, 19> H_gps; H_gps.Fill(0); @@ -262,19 +264,19 @@ BLA::Matrix<20, 1> StateEstimator::runGPSUpdate(BLA::Matrix<3, 1> gpsPos, BLA::M H_gps.Submatrix<3, 3>(3, QMEKFInds::p_x - 1) = I_3; H_gps.Submatrix<3, 3>(6, 0) = -1.0f * quat2DCM(quatConjugate(q)) * skewSymmetric(v_b); - BLA::Matrix<9, 9> R = toDiag(stack(stack(Max10S_const::gpsVel_var, Max10S_const::gpsPos_var), Max10S_const::gpsVel_var)); + BLA::Matrix<9, 9> R = toDiag(vstack(vstack(Max10S_const::gpsVel_var, Max10S_const::gpsPos_var), Max10S_const::gpsVel_var)); return ekfCalcErrorInject(combined_sens, H_gps, h_gps, R); } else { - BLA::Matrix<6, 1> combined_sens = stack(gpsVel, gpsPos); + BLA::Matrix<6, 1> combined_sens = vstack(gpsVel, gpsPos); BLA::Matrix<6, 19> H_gps; H_gps.Fill(0); H_gps.Submatrix<3, 3>(0, QMEKFInds::v_x - 1) = I_3; H_gps.Submatrix<3, 3>(3, QMEKFInds::p_x - 1) = I_3; - BLA::Matrix<6, 1> h_gps = extractSub(x, stack(QMEKFInds::vel, QMEKFInds::pos)); + BLA::Matrix<6, 1> h_gps = extractSub(x, vstack(QMEKFInds::vel, QMEKFInds::pos)); - BLA::Matrix<6, 6> R = toDiag(stack(Max10S_const::gpsVel_var, Max10S_const::gpsPos_var)); + BLA::Matrix<6, 6> R = toDiag(vstack(Max10S_const::gpsVel_var, Max10S_const::gpsPos_var)); return ekfCalcErrorInject(combined_sens, H_gps, h_gps, R); } @@ -286,7 +288,7 @@ BLA::Matrix<20, 1> StateEstimator::runBaroUpdate(BLA::Matrix<1, 1> baro, float c BLA::Matrix<1, 1> h_baro = {std::pow(lps22_const::P0 * (1.0f + (lps22_const::L * lla(2))) / lps22_const::T0, -1.0f * lps22_const::g_e * lps22_const::M / (lps22_const::R * lps22_const::L))}; - float dP_dh = (-1.0f * lps22_const::g_e * h_baro(0, 0) * lps22_const::M) / (lps22_const::R * (lps22_const::T0 + lps22_const::L * lla(3))); + float dP_dh = (-1.0f * lps22_const::g_e * h_baro(0, 0) * lps22_const::M) / (lps22_const::R * (lps22_const::T0 + lps22_const::L * lla(2))); BLA::Matrix<3, 1> dh_decef = {cosd(lla(0)) * cosd(lla(1)), cosd(lla(0)) * sind(lla(1)), sind(lla(0))}; BLA::Matrix<3, 1> dP_decef = dP_dh * dh_decef; @@ -317,7 +319,7 @@ BLA::Matrix<20, 1> StateEstimator::ekfCalcErrorInject(BLA::Matrix &sens BLA::Matrix<3, 1> alpha = extractSub(postErrorState, QMEKFInds::smallAngle); BLA::Matrix old_q = extractSub(x, QMEKFInds::quat); - BLA::Matrix<4, 1> q = quatMultiply(old_q, smallAngleRotVec2Quat(alpha)); + BLA::Matrix<4, 1> q = quatMultiply(old_q, smallAnglerotVec2dQuat(alpha)); x(0) = q(0); x(1) = q(1); @@ -343,3 +345,34 @@ BLA::Matrix<20, 1> StateEstimator::ekfCalcErrorInject(BLA::Matrix &sens return x; } +BLA::Matrix<4, 1> StateEstimator::getNEDOrientation(BLA::Matrix<3, 3> &dcm_ned2ecef) { + // TODO inefficient, use quat mult + return dcm2quat(~dcm_ned2ecef * quat2DCM(extractSub(x, QMEKFInds::quat))); +} + +BLA::Matrix<3, 1> StateEstimator::getNEDPosition(BLA::Matrix<3, 3> &dcm_ned2ecef, BLA::Matrix<3, 1> launch_ecef) { + return ecef2nedVec(extractSub(x, QMEKFInds::pos), launch_ecef, launch_dcmned2ecef); +} + + +// TODO for all of these +float getLastAttProp() { + // TODO + return 0.0f; +} + +float getLastAttUpdate() { + // TODO + return 0.0f; +} + +float getLastPVProp() { + // TODO + return 0.0f; +} + +float getLastPVUpdate() { + // TODO + return 0.0f; +} + diff --git a/src/qmekf.h b/src/qmekf.h index b29d3d6..08838ab 100644 --- a/src/qmekf.h +++ b/src/qmekf.h @@ -54,7 +54,7 @@ BLA::Matrix<1, 1> baroBias = {bb_z}; /** * @name QMEKFStateEstimator * @author QMEKF team - * @brief Attitude and Position/Velocity estimation. See matlab simulation for details + * @brief Integrated Attitude and Position/Velocity estimation. See matlab simulation for details */ class StateEstimator { @@ -89,6 +89,8 @@ class StateEstimator { template BLA::Matrix<20, 1> ekfCalcErrorInject(BLA::Matrix &sens_reading, BLA::Matrix H, BLA::Matrix h, BLA::Matrix R); + BLA::Matrix<4, 1> getNEDOrientation(BLA::Matrix<3, 3> &dcm_ned2ecef); + BLA::Matrix<3, 1> getNEDPosition(BLA::Matrix<3, 3> &dcm_ned2ecef, BLA::Matrix<3, 1> launch_ecef); private: // Identity Matrices BLA::Matrix<20, 20> I_20 = BLA::Eye<20, 20>(); @@ -115,6 +117,15 @@ class StateEstimator { BLA::Matrix<3, 1> sumAccel; BLA::Matrix<3, 1> sumMag; + + float getLastAttProp(); + float getLastAttUpdate(); + float getLastPVProp(); + float getLastPVUpdate(); + + + + // //R matricies // BLA::Matrix<3, 3> R_accel = {accel_var, 0, 0, // 0, accel_var, 0, From bbeb3754ac006b8e58db78818128d10f88a8b167 Mon Sep 17 00:00:00 2001 From: Abhay Mathur <66543532+abhayma1000@users.noreply.github.com> Date: Wed, 14 Jan 2026 13:50:24 -0500 Subject: [PATCH 11/14] More developments --- src/Conventions.txt | 7 +++ src/QuaternionUtils.cpp | 62 +++++++++++++++++++-- src/QuaternionUtils.h | 25 +++++++++ src/kfConsts.h | 116 +++++++++++++++++++++++++++++++++++++++- src/qmekf.cpp | 35 ++++++++++-- src/qmekf.h | 21 ++++++-- 6 files changed, 252 insertions(+), 14 deletions(-) diff --git a/src/Conventions.txt b/src/Conventions.txt index 6a634ec..ecd6297 100644 --- a/src/Conventions.txt +++ b/src/Conventions.txt @@ -20,6 +20,9 @@ Frequencies variable in format: baro update +Note: Could account for distance between the vimu and gps/baro for position calculations, but honesely, is wastes more cpu time than its worth + + @@ -31,4 +34,8 @@ BLA library: 4, 5, 6,} * Multiplication, addition is just *, +, / (note: ensure by a float. ex: 2.0f) * Submatrix: A.Submatrix<2,2>(1,0); Of a 3x2 mat A, returns bottom 2 rows +* Transpose: A: ~a +* Inverse: A: BLA::Inverse(A) +* PINV: A: QuaternionUtils::pinv(A) + diff --git a/src/QuaternionUtils.cpp b/src/QuaternionUtils.cpp index 3966123..1cff4bf 100644 --- a/src/QuaternionUtils.cpp +++ b/src/QuaternionUtils.cpp @@ -43,7 +43,7 @@ BLA::Matrix<3,1> QuaternionUtils::getRightVector(const BLA::Matrix<3,3> &rot) { } -// Works +// TODO redo BLA::Matrix<3, 3> QuaternionUtils::quat2DCM(const BLA::Matrix<4, 1> &quat) { float w = quat(0); float x = quat(1); @@ -70,6 +70,12 @@ BLA::Matrix<3, 3> QuaternionUtils::quat2DCM(const BLA::Matrix<4, 1> &quat) { return rot; } +// TODO dont be lazy and write +BLA::Matrix<3, 3> QuaternionUtils::quat2DCMInv(const BLA::Matrix<4, 1> &quat) { + // TODO one day rewrite it to make it more efficient + return ~quat2DCM(quat); +} + // Works BLA::Matrix<3, 3> QuaternionUtils::skewSymmetric(const BLA::Matrix<3, 1> &vec) { // Can rewrite as hstack @@ -218,6 +224,30 @@ BLA::Matrix<3, 1> QuaternionUtils::ned2ecefVec(const BLA::Matrix<3, 1> &ned_meas return R_ET * ned_meas + launch_ecef; } +// TODO test +BLA::Matrix<3, 1> quat2RPY(const BLA::Matrix<4, 1> &p) { + // Not matlab, but from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + // XYZ RPY + BLA::Matrix<3, 1> RPY = {0, 0, 0}; + + // roll (x-axis rotation) + double sinr_cosp = 2 * (p(0) * p(1) + p(2) * p(3)); + double cosr_cosp = 1 - 2 * (p(1) * p(1) + p(2) * p(2)); + RPY(0) = std::atan2(sinr_cosp, cosr_cosp); + + // pitch (y-axis rotation) + double sinp = std::sqrt(1 + 2 * (p(0) * p(2) - p(1) * p(3))); + double cosp = std::sqrt(1 - 2 * (p(0) * p(2) - p(1) * p(3))); + RPY(1) = 2 * std::atan2(sinp, cosp) - M_PI / 2; + + // yaw (z-axis rotation) + double siny_cosp = 2 * (p(0) * p(3) + p(1) * p(2)); + double cosy_cosp = 1 - 2 * (p(2) * p(2) + p(3) * p(3)); + RPY(2) = std::atan2(siny_cosp, cosy_cosp); + + return RPY; +} + // Works BLA::Matrix<4, 1> QuaternionUtils::quatConjugate(const BLA::Matrix<4, 1> &p){ BLA::Matrix<4, 1> quat; @@ -255,6 +285,11 @@ BLA::Matrix<3, 1> QuaternionUtils::normal_i_ecef(const BLA::Matrix<3, 3> dcm_ned return ~dcm_ned2ecef * normal_ned; } +BLA::Matrix<3, 1> QuaternionUtils::sun_i_ecef(float t_utc) { + BLA::Matrix<3, 1> tmp = {0, 0, 0}; +} + + BLA::Matrix<3, 3> QuaternionUtils::vecs2mat(const BLA::Matrix<3, 1> v1, const BLA::Matrix<3, 1> v2, const BLA::Matrix<3, 1> v3) { // TODO eventually replace with just using hstack BLA::Matrix<3, 3> ret = {v1(0, 0), v2(0), v3(0, 0), @@ -294,11 +329,10 @@ BLA::Matrix<4, 1> QuaternionUtils::triad_BE(const BLA::Matrix<3, 1> v1_b, const } -BLA::Matrix<4, 1> QuaternionUtils::quest_BE(const BLA::Matrix<3, 100> local_vecs, const BLA::Matrix<3, 100> ref_vecs, BLA::Matrix weights) { - // Note: IF we have multiple vec measurements, there are tradeoffs between: +BLA::Matrix<4, 1> esoq2_EB(const BLA::Matrix<3, 4> v_b, const BLA::Matrix<3, 4> v_i) { // Certifiable optimality, speed (microprocessor wise), determinism, and robustness to outliers - // Here I implement quest for 3+ vector meas, but read this and do research if other alg is desired: - // https://pdfs.semanticscholar.org/d30c/b21689c63660ca1de4c9c52ef3ae5b92e19b.pdf (chinese ppl unfortunately) + BLA::Matrix<4, 1> q_EB = {1, 0, 0, 0}; + return q_EB; } // TODO test @@ -435,6 +469,24 @@ float QuaternionUtils::sind(float degs) { return sin(degs * (pi / 180.0f)); } +BLA::Matrix<3, 1> QuaternionUtils::combine_variances(const BLA::Matrix<3, 2> &A, const BLA::Matrix<2, 1> w) { + BLA::Matrix<3, 1> output_variance; + + for (int i = 0; i < 3; i++) { + output_variance(i) = w(0) * w(0) * A(i, 0) + w(1) * w(1) * A(i, 1); + } + return output_variance; +} + +BLA::Matrix<3, 1> QuaternionUtils::fuse_measurements(const BLA::Matrix<3, 2> &A, const BLA::Matrix<2, 1> w) { + BLA::Matrix<3, 1> output_val; + + for (int i = 0; i < 3; i++) { + output_val(i) = w(0) * A(i, 0) + w(1) * A(i, 1); + } + return output_val; +} + diff --git a/src/QuaternionUtils.h b/src/QuaternionUtils.h index d24860a..5c57e8b 100644 --- a/src/QuaternionUtils.h +++ b/src/QuaternionUtils.h @@ -11,6 +11,8 @@ namespace QuaternionUtils { // Quaternion to DCM BLA::Matrix<3, 3> quat2DCM(const BLA::Matrix<4, 1> &quat); + BLA::Matrix<3, 3> quat2DCMInv(const BLA::Matrix<4, 1> &quat); + // Get the up vector from the rotation matrix for the payload // In NED coordinates, this will be the second column of the rotation matrix BLA::Matrix<3,1> getUpVector(const BLA::Matrix<3,3> &rot); @@ -44,26 +46,48 @@ namespace QuaternionUtils { BLA::Matrix<3, 1> ecef2nedVec(const BLA::Matrix<3, 1> &ecef_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET); BLA::Matrix<3, 1> ned2ecefVec(const BLA::Matrix<3, 1> &ned_meas, const BLA::Matrix<3, 1> &launch_ecef, const BLA::Matrix<3, 3> &R_ET); + + BLA::Matrix<3, 1> quat2RPY(const BLA::Matrix<4, 1> &p); //Quaternion Conjugate BLA::Matrix<4, 1> quatConjugate(const BLA::Matrix<4, 1> &p); BLA::Matrix<3, 1> qRot(const BLA::Matrix<4, 1> &q, BLA::Matrix<3, 1> vec); + BLA::Matrix<3, 1> qInvRot(const BLA::Matrix<4, 1> &q, BLA::Matrix<3, 1> vec); + BLA::Matrix<3, 1> g_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef); BLA::Matrix<3, 1> m_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef); BLA::Matrix<3, 1> normal_i_ecef(const BLA::Matrix<3, 3> dcm_ned2ecef); + // One day... + BLA::Matrix<3, 1> sun_i_ecef(float t_utc); + + // One day... + BLA::Matrix<2, 1> earth_horizon(); + BLA::Matrix<3, 3> vecs2mat(const BLA::Matrix<3, 1> v1, const BLA::Matrix<3, 1> v2, const BLA::Matrix<3, 1> v3); BLA::Matrix<4, 1> triad_BE(const BLA::Matrix<3, 1> v1_b, const BLA::Matrix<3, 1> v2_b, const BLA::Matrix<3, 1> v1_i, const BLA::Matrix<3, 1> v2_i); + // TODO one day implement when have more vector observations + BLA::Matrix<4, 1> esoq2_EB(const BLA::Matrix<3, 4> v_b, const BLA::Matrix<3, 4> v_i); + BLA::Matrix<3, 1> lla2ecef2(BLA::Matrix<3,1> lla); BLA::Matrix<3, 1> ecef2lla(BLA::Matrix<3, 1> ecef); + BLA::Matrix<3, 1> combine_variances(const BLA::Matrix<3, 2> &A, const BLA::Matrix<2, 1> w); + + BLA::Matrix<3, 1> fuse_measurements(const BLA::Matrix<3, 2> &A, const BLA::Matrix<2, 1> w); + + template + BLA::Matrix pinv(const BLA::Matrix &A) { + return BLA::Inverse(~A * A) * ~A; + } + template BLA::Matrix extractSub(const BLA::Matrix &x, const BLA::Matrix inds) { BLA::Matrix sub; @@ -180,4 +204,5 @@ namespace QuaternionUtils { } Serial.println("}\n"); } + } diff --git a/src/kfConsts.h b/src/kfConsts.h index c34dd9a..84963b7 100644 --- a/src/kfConsts.h +++ b/src/kfConsts.h @@ -6,14 +6,126 @@ using namespace QuaternionUtils; -constexpr static float g = 9.80665; // [m/s/s] Earth's Grav Accel TODO eventually take out when have wgs model -// TODO: Fill out this file with correct terms and our units constexpr float square(float a) { return a * a; } + +namespace earth_consts { + // Baro: + constexpr float P0 = 101325.0; // [P] trad sea level (word on the street) + constexpr float L = -0.0065; + constexpr float T0 = 288.15; + constexpr float M = 0.0289644; + constexpr float R = 8.3144598; + + // Grav: (until world model implemented) + constexpr float g_e = 9.80665; // [m/s^2] + + // TODO will want better geodesy if going high up or using inertial mindset + BLA::Matrix<3, 1> earth_rot_ecef = {0, 0, 0.00007292}; // rad/s + +} + namespace mars_const { // BLA::Matrix<3, 3> R_BM = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // BLA::Matrix<3, 1> p_MB_B = {-0.5, 0.1, } // No, TODO: Do as dual quaternions + + BLA::Matrix<3, 1> bodyToMarsDis = {-0.3, 0.1, 0.1}; + BLA::Matrix<4, 1> bodyToMarsOrientation = {1, 0, 0, 0}; + + + // ASM IMU: + BLA::Matrix<3, 1> marsToASMDis = {0.1, 0.1, 0.1}; + BLA::Matrix<4, 1> marsToASMOrientation = {1, 0, 0, 0}; + BLA::Matrix<3, 1> bodyToASMDis = bodyToMarsDis + qRot(bodyToMarsOrientation, marsToASMDis); + BLA::Matrix<4, 1> bodyToASMOrientation = quatMultiply(bodyToMarsOrientation, marsToASMOrientation); + + // ASM gyro + BLA::Matrix<3, 1> asm_gyro_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; + BLA::Matrix<3, 1> asm_gyro_bias_var = {0, 0, 0}; + + // ASM accel + BLA::Matrix<3, 1> asm_accel_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; + BLA::Matrix<3, 1> asm_accel_bias_var = {0, 0, 0}; + + // ASM mag + BLA::Matrix<3, 1> asm_mag_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; + + + + // ICM IMU: + BLA::Matrix<3, 1> marsToICMDis = {0.1, -0.1, -0.1}; + BLA::Matrix<4, 1> marsToICMOrientation = {1, 0, 0, 0}; + + // ICM gyro + BLA::Matrix<3, 1> icm_gyro_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; + BLA::Matrix<3, 1> icm_gyro_bias_var = {0, 0, 0}; + + // ICM accel + BLA::Matrix<3, 1> icm_accel_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; + BLA::Matrix<3, 1> icm_accel_bias_var = {0, 0, 0}; + + // ICM mag + BLA::Matrix<3, 1> icm_mag_var = {square(0.0020f), square(0.0014f), square(0.0014f)}; + + + + + // LPS Baro: + + BLA::Matrix<3, 1> marsToLPSDis = {0, 0, 0}; + BLA::Matrix<1, 1> LPS_var = {square(2.0f)}; + + + // GPS: + BLA::Matrix<3, 1> marsToGPSDis = {0.1, -0.1, -0.1}; + BLA::Matrix<3, 1> gps_pos_var_ned = {square(0.0020f), square(0.0014f), square(0.0014f)}; + BLA::Matrix<3, 1> gps_vel_var_ned = {0, 0, 0}; + + + // VIMU: + // TODO one day model bias instability + + // VIMU Accel: + BLA::Matrix<2, 1> accel_avg_vars_inv_tmp = {1.0f / icm_accel_var(0), 1.0f / asm_accel_var(0)}; + BLA::Matrix<2, 2> accel_avg_vars_inv = toDiag(accel_avg_vars_inv_tmp); + BLA::Matrix<3, 2> R = hstack(bodyToASMDis, bodyToASMDis); + + BLA::Matrix<2, 1> ones = {1, 1}; + BLA::Matrix<3, 1> r_bar = R * accel_avg_vars_inv * ones; + BLA::Matrix<3, 3> M = R * accel_avg_vars_inv * ~R; + BLA::Matrix<3, 3> M_pinv = pinv(M); + BLA::Matrix<2, 1> w_hat = accel_avg_vars_inv * (ones - ~R * (M_pinv * r_bar)); + BLA::Matrix<2, 1> w_accel = w_hat / sum(w_hat); + + BLA::Matrix<3, 1> combined_accel_var = combine_variances(hstack(icm_accel_var, asm_accel_var), w_accel); + BLA::Matrix<3, 1> bodyToVIMUDis = w_accel(0) * bodyToASMDis + w_accel(1) * bodyToASMDis; + + // VIMU Gyro: + + BLA::Matrix<2, 1> gyro_avg_vars_inv_tmp = {1.0f / icm_gyro_var(0), 1.0f / asm_gyro_var(0)}; + BLA::Matrix<2, 2> gyro_avg_vars_inv = toDiag(gyro_avg_vars_inv_tmp); + BLA::Matrix<2, 1> w_gyro = gyro_avg_vars_inv_tmp / sum(gyro_avg_vars_inv_tmp); + + BLA::Matrix<3, 1> combined_gyro_var = combine_variances(hstack(icm_gyro_var, asm_gyro_var), w_gyro); + + + // VIMU Mag (Same as gyro): + + BLA::Matrix<2, 1> mag_avg_vars_inv_tmp = {1.0f / icm_mag_var(0), 1.0f / asm_mag_var(0)}; + BLA::Matrix<2, 2> mag_avg_vars_inv = toDiag(mag_avg_vars_inv_tmp); + BLA::Matrix<2, 1> w_mag = mag_avg_vars_inv_tmp / sum(mag_avg_vars_inv_tmp); + + BLA::Matrix<3, 1> combined_mag_var = combine_variances(hstack(icm_mag_var, asm_mag_var), w_mag); + + + // VBaro: + + // TODO one day when we have a lot of barometers. Same thing tho + // BLA::Matrix<1, 2> vbaro_weights = {0, 0}; + // BLA::Matrix<3, 1> marsToVBaroDis = {0, 0, 0}; + + } namespace asm330_const { diff --git a/src/qmekf.cpp b/src/qmekf.cpp index 0837064..84109a5 100644 --- a/src/qmekf.cpp +++ b/src/qmekf.cpp @@ -15,7 +15,7 @@ void StateEstimator::init(BLA::Matrix<3, 1> ECEF, float curr_time) { launch_ecef = ECEF; } - x = {0, 0, 0, 0, + x = {1, 0, 0, 0, 0, 0, 0, launch_ecef(0), launch_ecef(1), launch_ecef(2), vimu_const::gyro_bias(0), vimu_const::gyro_bias(0), vimu_const::gyro_bias(0), @@ -125,7 +125,7 @@ BLA::Matrix<19, 19> StateEstimator::ekfPredict(float curr_time) { // TODO finish this. too lazy BLA::Matrix<4, 1> att_last_relevant_times = {0, 1, 3, 4}; BLA::Matrix<4, 1> pv_last_relevant_times = {1, 4, 5}; - // float att_dt = curr_time - vecMax(extractSub(lastTimes, att_last_relevant_times)); + float att_dt = curr_time - vecMax(extractSub(lastTimes, att_last_relevant_times)); float pv_dt = curr_time - vecMax(extractSub(lastTimes, pv_last_relevant_times)); @@ -303,6 +303,14 @@ BLA::Matrix<20, 1> StateEstimator::runBaroUpdate(BLA::Matrix<1, 1> baro, float c return ekfCalcErrorInject(baro, H_baro, h_baro, R); } +void StateEstimator::setTemp(float new_temp) { + curr_temp = new_temp; +} + +float StateEstimator::getTemp() { + return curr_temp; +} + template BLA::Matrix<20, 1> StateEstimator::ekfCalcErrorInject(BLA::Matrix &sens_reading, BLA::Matrix H, BLA::Matrix h, BLA::Matrix R) { BLA::Matrix residual = sens_reading - h; @@ -350,10 +358,22 @@ BLA::Matrix<4, 1> StateEstimator::getNEDOrientation(BLA::Matrix<3, 3> &dcm_ned2e return dcm2quat(~dcm_ned2ecef * quat2DCM(extractSub(x, QMEKFInds::quat))); } -BLA::Matrix<3, 1> StateEstimator::getNEDPosition(BLA::Matrix<3, 3> &dcm_ned2ecef, BLA::Matrix<3, 1> launch_ecef) { - return ecef2nedVec(extractSub(x, QMEKFInds::pos), launch_ecef, launch_dcmned2ecef); +BLA::Matrix<3, 1> StateEstimator::getNEDPositionBody(BLA::Matrix<3, 3> &dcm_ned2ecef, BLA::Matrix<3, 1> launch_ecef) { + // Returns the distance from + return ecef2nedVec(extractSub(x, QMEKFInds::pos) - qRot(extractSub(x, QMEKFInds::quat), mars_const::bodyToVIMUDis), launch_ecef, dcm_ned2ecef); +} + +BLA::Matrix<3, 1> StateEstimator::getBodyAngularVel() { + return gyro_prev; } +BLA::Matrix<3, 1> StateEstimator::getVIMUAccel() { + return accel_prev; +} + +float StateEstimator::getGs() { + return BLA::Norm(getVIMUAccel()); +} // TODO for all of these float getLastAttProp() { @@ -376,3 +396,10 @@ float getLastPVUpdate() { return 0.0f; } +// GPS is less accurate in D direction, so account for that here. Vel then pos +BLA::Matrix<6, 1> getGPSVar(BLA::Matrix<3, 3> dcm_ned2ecef) { + BLA::Matrix<6, 1> tmp = {0, 1, 2}; + return tmp; + // return dcm_ned2ecef * gps_var; + +} \ No newline at end of file diff --git a/src/qmekf.h b/src/qmekf.h index 08838ab..c43497b 100644 --- a/src/qmekf.h +++ b/src/qmekf.h @@ -66,6 +66,8 @@ class StateEstimator { // Error Covariance Allocation TODO eventually BLA::Matrix<19, 19> P; + float curr_temp; + /** * @name init * @author @frostydev99 @@ -85,13 +87,26 @@ class StateEstimator { BLA::Matrix<20, 1> runMagUpdate(BLA::Matrix<3, 1> m_b, float curr_time); BLA::Matrix<20, 1> runGPSUpdate(BLA::Matrix<3, 1> gpsPos, BLA::Matrix<3, 1> gpsVel, bool velOrientation, float curr_time); BLA::Matrix<20, 1> runBaroUpdate(BLA::Matrix<1, 1> baro, float curr_time); + void setTemp(float curr_temp); + float getTemp(); template BLA::Matrix<20, 1> ekfCalcErrorInject(BLA::Matrix &sens_reading, BLA::Matrix H, BLA::Matrix h, BLA::Matrix R); BLA::Matrix<4, 1> getNEDOrientation(BLA::Matrix<3, 3> &dcm_ned2ecef); - BLA::Matrix<3, 1> getNEDPosition(BLA::Matrix<3, 3> &dcm_ned2ecef, BLA::Matrix<3, 1> launch_ecef); - private: + + // Return the position in the body frame + BLA::Matrix<3, 1> getNEDPositionBody(BLA::Matrix<3, 3> &dcm_ned2ecef, BLA::Matrix<3, 1> launch_ecef); + BLA::Matrix<3, 1> getBodyAngularVel(); + + BLA::Matrix<3, 1> getVIMUAccel(); + + // TODO figure out exactly how to implement this. + bool isEKFDiverging(); + + float getGs(); + + private: // Identity Matrices BLA::Matrix<20, 20> I_20 = BLA::Eye<20, 20>(); BLA::Matrix<19, 19> I_19 = BLA::Eye<19, 19>(); @@ -123,7 +138,7 @@ class StateEstimator { float getLastPVProp(); float getLastPVUpdate(); - + BLA::Matrix<6, 1> getGPSVar(BLA::Matrix<3, 3> dcm_ned2ecef); // //R matricies From f3a467d01de104940d3e1998034709e8e87d6d34 Mon Sep 17 00:00:00 2001 From: Abhay Mathur <66543532+abhayma1000@users.noreply.github.com> Date: Fri, 23 Jan 2026 15:27:34 -0500 Subject: [PATCH 12/14] allan var and standard var stuff --- allanVar/allanVar.mlx | Bin 0 -> 136268 bytes allanVar/standardVariance.mlx | Bin 0 -> 3486 bytes 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 allanVar/allanVar.mlx create mode 100644 allanVar/standardVariance.mlx diff --git a/allanVar/allanVar.mlx b/allanVar/allanVar.mlx new file mode 100644 index 0000000000000000000000000000000000000000..bdee812c5c5429fdb76b88e73863bfc656c95b7b GIT binary patch literal 136268 zcmY(JLy#^^u%_E~pSEq=wr$(CZQHhu({`V>ZQI5-|DBo5U1TnDQxQ+R@n&UKDawF? z{Q&|3g8DCrfHubXD%pU6fa*YjfKdOd=!iJjyPDa%8mM|Xnz`uFd)nEyrOe0=Ga`lF z`N2$Sq;@(ALXq7B5k$d?R`L!U+U8Wo+im2q`}wfTQGp|XLEAgIpXEG=PM+$Sj6jIg z=FOaVai@XQxtrN(bopv+Q)noQg6IT0p}TYqQ;zzwPoncAC^c@5>8gwVYFAa#*bbr2 z2Ll3v{g1MNvze_61O5Nxx|G5H=)wu!{X|o~vDt8t3)*>rRB?okL1vEF)U-*ZmfC*# zFb9bb7gD2+Pdq%i;96XXtR-`>wkt7LNby#Hk!chssmU9#C$#8D*Md?Ns-d`ei^@!P zM>bp74t|j8aBQL|B(YbtxqRj`P1#%zj7gU~C)F|IaR#uWEe&`m63LuBp|OZS&=CuR z&{B!X%Tb|7vl~*oa=qlD>t`wU+$naiFZtb9J`}h2Yu%-7Y-HBN`<;G_$(N;wX?{+Z zTx&!<3k$!aBH;lSpDfFNtPT+Hq#-y_)Tg?O}1ZQ!InYLCiziU-$|^p4L7~fZsjW%w#z`aWlVsNruu>qtD#(n1{K`VQ%xX3+i#;R! zZ=k^>J7Q-4!CZm=f3meRaYs!~{_Zxgl#? z!h_gtNoro}IAT(Wjj}be=~KhIh?5dH`>Xssz~UoFq&YL>#V6{j03XycHRJ}x6{lUs38$7=v#3q!BH@s-DpyVQMMFyI zB6soZMx##QYSGjKZA+?ue@fEmmSIyW>_!A+pZ8;?92DaB@C?j{U^v0zh)Y7-wm<#U z#j3R4LaH>-I*+|pKaQZ&H;~3kJ2aOGs`^RWEKnE8m6)^j_3-$zY6q9|FCahsqNA^2 z^rT$1P{}_U)u7UG@#xykv+rTCk4^iBX zW3!caSv_ssM`@w zGm=$Z>#}5tE#?3zOFJ9BUo6R+%YszTr2PlqDu$@D=m}d`0Pag(#00(O*TfDK$4bNT zx|+wK=5OJp|C(F^TMF?iZHv5I%x=7Y{!{WY(61l*Q$X`a{%tIm>j3#+e-N;_GNz{v zHVwM6)ZecJ6`{xe;~;Q{G*I2a=4?#ayrA$78*oZ7r#Xmm{ z<_v`ionJU;djK(>Px$}sN;uMlb{r5OK#h+eK*;~u{Qq|)2RBzoxBu%&xnFC^*VFBJ z(0wp*9hT^=)w{fAP?mKkc&$*DXh8XWNdWT{UNwy!t9Eq=BqW44uJP}1o<{?~8rw7A zgm$eiZw&BjZ~y@PYygzuK2=$cfA#}3_E3LuUJL*YfOicpQ!}O1Ft6*UCI`SnL(#Sh z{m#-`4a-e}Yk-2sn=1prL~@+u1_1uM_Xem@xVu;f^giqseq+4%-+in-&y@k#<8K63 zndxo=f&f-~{q0>2Z@s#Jap))RnWayU5J2EB;EXTyAzy$dkw zKYQ#4tUcd71^5+|GDJ9U1C9&={sKy@uT1=KKIX;%oL74q?{6H=4+K{6YGB}LfH+y2 z%YGwO;Gd4xp(a3hk(F4Yn0m6~3&VW?{;z=i^V+LG^qVlif~V9Cde-CzFuPbs+6d6V z@s&M;tTN_fx=T3tguh$6t2lAck<}vi4{%8igu2)RmNo4}AZ57-WPVb@)xVbs z+Lp%|4R+Af#9OVd{TKZh%|t{0YpRy})QR!u-&>2ImMVif z$s*-)A)B2{vmnENsv<7L7?l%vB=KkzE(+=3Mb6` zGq+tfm3o<*dP%&tgm6+fnw?iY3&G8;jD)#q{((-8)86S5+mqW>=`A~tnlc`!fB7Br zb;QTS-Wy@~2M@22w#NbHSAf77U-(aG-H_~0?alG{M}Y1dU&Ytl+XvtTH#!afJN&|N ztYOP;E|`B_mQpHP-Q|8#358FFurP5EX4Wr2g>0HUnUsMcnpKPnV%|rR_aOMe%HPq< zO<)nE?YEo+H)6qQ#$%ZxAWlZbJzQT8>M|?uCsFo7$8*qGjX1vvjGTK%q$fgitS#%t1{OBUax2rk&D-evbQ+!W7LtxT9(!v!KP7(p{`T zzwC!F@Gkr+e=mUq4hnlL?6r&kCOz3FKcXc)5hBW8@2hVQqqbAlA93Lc&chg3$PgHG zAekF)C2+Lj;`M4sI3a;~mGNVA%|8;CbZ6Unj#5_Tn2RBDHS2^+4A?bn79Xa%ZZ{82 zGG(%5D#(>p;7f}p5tqieS}-N)vwBxum+52ZZr6Mlh?JLuMmh>PU&%c*9=+}y$%Sr? zujk9D8LmY+$0Jf<2INs}NC=6}>Tbpq-8PBM+d?&w5??II472_(BSO5=mPe?cKs1bn zEq5OR@{%3Dt|J@X8tdbkUBK%BjsUppN~SHAc_1R9QYt~wMEKGmSlQT)?BXgoN4>{E z(^PWkuRoDF@_K8Yk&8N|a!-VZ9I60wbf4c-YV zXDhPQWUVBZ9cS-{FBL(Tk*xSHeFV}~{aqvqiAMUClp|w2Wh|%7&^0!6Fj=E9rg**H z2%bqRghi+TrCx9vcHV%(E1xT-=#+#|t6&n)ZT4sL)f>@!dO5t94;jYfKQSf8ZT9mk zjg+e|7nDXQ&~Q`=OptOF`0jp-9a;p9~~uvX+WF*;NOjO zE$&S=r*4<*5k9vs-{3)(@3msHbiPh<9Go~x#Y#)nlTsHHv#o*b$FK_s^;jN$7LwnO^Ebi^da zrluCbnEVAMo>;^{7k?!P3N{!l-RW|%7o&zU9E@PCFIRyLX}_zU2w_#sF+CNxBnAxS zspdzD0DAj9bYGwiY+gfuJqrwoxdQY6s5FFnHU;rCL1yUO>7=)&d zR;pf+eWY~CkCcOpiG|NK+w=+IAq1>Xy)!dQF~s2K_s5zT!6(T!^A$`7o^&(5+#k06 z)wh&99;r$IdSy6&tdX-2m89t{1gG+>@||E$m>|*vJfpV2Pt6w1-I}~cV{AGGXDWo@ zqX!_~VG|EifcRw}wV9Z@1yOFqRZgL7S}U$y(@K4RNz5!=S!1`gL?p~=yA%cnKmNw2 zzJf%svLs?M%s=dyc*F?j_7*^I?wBm%bF?mu2~x#8&YOg)`|CqrIiWcn$rothS$3 zY{Pc;MzT{+jsI^1-F5T@7#gJ!(AZ&czY?=q*BQGSRS$YUTy7)R|8CeEUdQB+v-R{F z`VezSDA_PtobS?GO4k(4MS@zq1MBu^h;%FaMB6)L{;r();dSnxEK||)#)~nnib0Cb zvWlAPnJ4c7^kn-CH&dOZ3?$K$zsvAQv-f~YSTqB5;?j>7sl*`X0%j#2-|KPB->*S& zou)igS2VMGw$6=@*%}gLcNkt*Zxj2L*@t#Zai<0WCGmJ-cql`a^Us8RC35#ec?Gkn zDROY@b=0ZaBSr}ceii;$G>~ZGO%3_$8^R&T>umeaAB#0E+e|>FrG83A z(1@==NP7J1jCFy;2H5KgjxZB3Hb|^%w2$RqZeoSwMhmuZ(k5LQ*Q+x_y2FD^?po`f z-ZDVlkJhhu#*$Cw&t>j3BGd7UQRb7dY?6ry5*rW{Y$v)EC#{)`U?n= zdDXLci{c1eKBhrkO@qJ+YJKR8V+XR#PA*MQ$#tNNJzRUY$%*>*TJ`Y3Ghsi|svi~_ z-M7R7p(i#D63)Bgro%5WssAI6$Aas8rr_|n%M?@jp&Oy4*73U+K@csd37!aR{1a9F zQMb9Ey+7mzn3gCO!*u-J-IUT<6rs0+6<)-rE#|PU9{l0zL#3?s$ zjOx7pUzb86Uz z30$2}dc+wXs_w6K$Oa3N-5l`Vxc9#dx0kb@k)(TqNbeU`uCuXP$1oA7mDYR&#KY*L zGl`;CFA~_pvbI5!?0h1W^)eUvci(Gv`AT*bdQ<`k`#{M8TL!W~vwlw|e!9m|jSz=X zLX-%S*>;#I2LE1sYVPwU!_2u zkjr3cimf~|4>)bZy}*%fVdHha5F~+}gzAw)Lt5T?6B_rogAk~TQfNjwyV37e@#83t z&=hqD$^l!E=!SYMZNHz1R3cyG)=mNfFc#nfw0R3guJpnW+{q~!f_`)MmGo2mcGEdd zCSuUtV>J+c^c4ZWa61Un$gc$tP28DsyzzGMT}tM(43mw=eE+4D%dN3*6wKCmBSFbF z9Cylp$KPzW%mhT{ZM;l!+ou8fYo1=lpZ7xC_Io^MYbk4`gRh?*`l}&p>gID!h}Yxt zv=q-@#JZCFl@6s7Xec$Aluhs{Z9F4XVyicgNALR>U;UXj+I2t<1u}Vj zQkT)iu4thNfy@C{WgSF_vt3dKs1l5mx8KV;yU5#^M}?MWQr-HSwy|TfsO6`E-7#Qi z>~zFBB~%I~cTd1<09iPodR@!s-DVX5Qr9-?uKpoCsU3 zeIqGkef+?_`>PH}o|a=`2gt=GD=SkadR0$`li#OI>M*>1?qnQZy>bV}l%dn!bVY}E zxliucnjXkCxc?@*BbgWZJ~x`IdAKoxrv3tdbqW*b3eT`rojfO=TS@A$VH1tUyjie`PgDz#pTO?9DavrL*ldVjEE7n)rb_PKH!7o zY|Jl+G-DLaxJq-Vd z1(g7%U4;5w>uc2(O-eup*@6uaA+y7$mRT9Vrt)iRpI6{CbTb7B=3O}mqDkqdE@x}e znVTrVganDfT(p3hI3_`I6c(r-W`d36jXFgnT|eer3bZ9r?bAJV^EE;9t#s#ub5wFS zM<0ql1X-P{wZZgl)o!t1dXE{u&QFruj^>9I6cpD)0<2@p!~!QIF|GVKZUqW82>Zkt ztN+^zfJXWSE{+f@vOqeTQqbpK2FH2FZ8fBO@_i9Im+4^LqEDS&<3=*_cbRI1_SC@u zD*{>?j{2Zx`nE&S>+5ePAz1X?NVm?RH-3KE5*8#&j3|*Qs|Zr5m^ib#uC-y5O#Cv` zQ%A>|q0I*2)S{>r1zGZP8x$I)SL79fHjEr)ifvp4RXSQpf={?q-00?DiP~^b4E=UN zczSs=iH<0#E*A0m3N@{a2%6YHe2%Jxb4%cQn( z)%+WYEX|C~>R+osFm%(u*?8q=b}>>k^edDC@@l2hA*ayiE5|>mDl03zT>XHYS2~R= zNbc+tN#XNpbn&bspW#E!T2#7i2x&s|jCwGMGs|O)Z{lOVlhD?TkCcV@iUt@6m7}@2 zW+@b@9kjgPyLTwD+>Ot}&4=#GfVi&0@Neog95bpI^wbHvvki0CRQEYCa6`{L;GvZJ zJvF20gZL1&ctAy~MYn)v>L^w-okKlY z;j(pL0&|)8Dx3cGPt!zSQ&Hp-%!9g}wdCQfu7bp=&Gct)HXZeDJNygnp~5B1;yBTk zC+A~#<8PO;@sj7mSQ(^}>=H-|S<>fKrS+zR#kMp%Fi9hQ)V`w$%2=2n7H7dP)4_h| zk>`d}39toSCxiX=4cDqHk z1uSCmeMK_twh#a_rPS2B)apkMBDuXtbJuh&2venQ5Qm^rJn4`N_qwWKlox>W{`@V0 zkB;0UHHJP;YZ56>MJ#UJbNbrwhz3A5-z(Gj8QEFkU;og3)SHIZGuJBXWgR3C&U7^5 zr23AH_8rPFZT6p10pm5^C>>Z2^8Hkf-#%#-4M#L>bWdLNT9ZBhO58P&{bYd`+#c?M zTIPF+V1}dNNwA8M-pK1M#_PY~?$ps@lC?u`*Qn!&0(Hr&##`-gobgk}_&{QO*31nI z#BMqrjB%3AVBMQx;t?jm40tp*s@=WwD}v87l_-06751UGK`Cu7J8jX@60h6vQ{MAz zggDgTSBfE%-)kgY^Vj=M^YkA~TV-YL(y&4t6`@%6mE))ZS6rq|!Ubskl$1paEYE?1 zNNVs;FYOTbDXF&nZ21cB1*m$E;HMrzAk)8L#E4)`T=>Ow;m$!()}p>$E4PAQY@oqF z542Y#G<@L>+tA9Q#+nOPVSx*)T`cVnRAS8lqM}ZS{@Nu8C4jF`+{gF)wB?my(G-)S zD;n@JHJ~;@*}&*~fAcY|mW`R%AA+)%m{ZHi_js);7n4f<&s8vjMmo|{Q6LR^M@C((R<7t`XQcg zy!-NYx(sx zd%lqnsrC>-kL|HmEGSC>J;~bRX{}kFx&XvZMu|1f1`vi>cp)I9>DJ#TXVY?9n71)d z3%3=lo^lDsLuifn`8QcR&fdja`z~nEU@Mff=Er0!qx$# z3CH??wyZS`ACqrY4{Jf9T2p=?+`dG$VhVaK&UqL;*V^^_)pLJv0sKVKOW(x-SBCxI z9Vqz(5;uhrXpCi81tA#qDA07}dG`yi>OB#N;ZWsKpAq8NuP9*#$Z-F<>Y0XKMMCFC zQL(75zO-B%F3{LjyomH7SFQ^h`A4gkUZRn)<)nvlP!wh~U#<8lJWu zaF*?FS_v>o_$Pyfl^f{(A;-w)@cy`ed{<=or`IU-o82e|GBYX^@_|luxO?cjxgq#N zSM71+t=J@fy055m8N46i-=$7ZwEVJV%^qS?@=|}AyxYv|TK;SEUk-xkc&5Y3_LcTu zPU>4&!41;^IDM~vqTkHFQ+rEO^M}7oEe&6G-*J>Fpcau(ssW2B7@#=PM9eJbWtDXatJHts!S?sLq4av^p{x|R zL2SB$bXyKC7~_Xi+fX+9m4tJ49==G)W;dVNKtHHbN~QgQ_E*}$Z4qWexM4W@7CJ!v z?qik)Xb#c16zT4Ukl#o`{HbDhmGkdHJ6`E$`^jPleLq?WuqC>ws($LyV!TPl(MD5X z{5G_A_Be$u!`2&-1VM*q4Wm^tNgIo}(||=ofPq#{S7>HGZzaN_62%$kCn>*q-s6+3-qjBEOGz^hk=T^pY61-SmFtS%Q#H3rFjDTfd z-AO0W!e|`P!Y!_n=?PQXlEJZ=AY>{OLcodZGXq`T>q?A+DcweCL7m z_u&e+QhY!11#6Cr4GpAUG{RB*E-TDb3*AnlV7{@30yW*gkl{^u;0U`psh}42Zy6Jk z2N)zww=VrJQ>-@rpJ1!~k75GLv*P}+8pz9HzqGX1+7ZwL!xzlg*(sSF zU-{?->rxM25t%WMms#mgufKdXY=+8KG&I*y1w2vwTT9NWGWzqk4TH$sMfDcVP0f?& z;rBcCpe4NKx_Nr_RN3ki%d{G?-f3$CM0^PUKUE%@E%zD6?jO zTP7?Gp0TZjOUGEd=e&20=U8_n|DsIG0F(;mF2!i-aeOhdZF6ybpT>N}WhW!AK};*Q zQ@jY%R0ig&j`;0G!tf)GNWtz|RD(uH89lTETWv~!1Uu6Kir=k2)_oQaUWAu(vhUKa z$i5_)F$`nNn(eOGi#8>22P%;kAVJm~z;KL0)U|yiKN#}{lZz%!GN1WEa)4Z}IbZ{#);p z{~3|l&-dT6xM)=Gbf31)KdY}>>%_qdj@5Qiq9Kbf3x-omGfpGxbK@XctJcp=k3Rez z6oxE+cogY||9%dumG>mkyL+#aY7yA9Su%_)UYE-yEOlsjX@47yagufk2F^v0mIz8# zJMpO%)Y47>)l4gp6JP75T{Ts~WBX(X0DC;esPDdJpYos|#`f3rF~G>jd4>7O$iLzs zh>Jae(e-snPh8S6Zr^xmD zO6t~o_<_jd27DX0ce0z{>3sRxgQlBiB~VT8qf4G@xd|f|P0c^JG>L_Iz4x@0*>9^Bz1oyQ(SQr9ScDjY22a<4k4zoDJ6vVm8Z%Ly!Rcy8+YFXSL_ zYry<*MA2SoLerqmer668C#y?VEFvp_2Gy=J)&PA_$=*5GIqUl_r(x6K^&DwlJ32sO z;R(%o^5ndG&JvwbA+}Q^0_vB$p7y|Wc)2g-bLVK90*iVUj zs`G>AxY3=L#)F%a(dXbcK`jDJ$W!q_W~)yF#j^&v9;{rc9oC)CvD$nBPg8kLhxf&M z|9-Hgsu)PPJqbu|j|u&IpHkm8ltf44uIM8M)}I+=6`4Fu$q?w78n8r$RkP7`GvQE> z+4TkAXN|)g!q(iepSVj>&v5k2JlFG`Qkh*l^#uv=+57LlF7vE!uD+^j=qR?VneQTZ~n|kLh-Bq;>NovA`@t#d$Ucos5A>xAg?2UPDH% z%oQ#ZGUpd5$Q)&%(<$-!r)Ff68pDEw$Y`(a4>5tXs@&&u(WY>y24$Xyq2LUOy8WoG zF0gKil9M`sUV8)HGX#q_=?HbeO7&k6e3N?Jsj7--Kn4c{(sh z0*Lp@R<5bN(BRfEh!M^}l$~U~mgz_qrKnQb#~}ksUaq;Y8M2mfWprOC{ADY$#zIVY zG^5xYu@-@uy)UnP5_w+Q+v+=8VG?-)onQI}k^0FDyyY(2nb9hP(=_oO^``U#{~jDp zR-Fe-1zEr`419LM3o0V4L}b+{`Yd(;Pv}P*Ccru2ki-3^yzS>v(LyP{x(xR%Ud#gQ z_2Z4$SBx+w(h$8W5-gkb}5TzULAi z2|dRTL|*k55CG!DSS|c11M$a}T6Z0qfKGDO8v!~mvPyGLq?t2aVj@WP%|!2b2wXhF z)?B6w*%3(2X*#V^R9PSWs4Hn`Iv*0mYvBH}d(hrV*&YO`wfC5bY2jRDZSc0q)<8(o zyp=Hv$lJP4n$L9F$|6Vr$o=n2o-QezRNERD#)p!w9?$b%Wg>+4N{!aL4wNX6egj+fL z92fBMoZ3NdI2c zbgK?{%5!TlUk6h#zY%;Z=kz*GT3`njn77}(;ZpYJ)h@s@@`ts#1f@%i#yA2rZX!DB zF~Q1hB(7aVu53l2yns(ip5Rxy-d772G}_j|aW zaGQ=rjbi#|VB^DJxc005T_z`7V@?IR0C-0qnnDSVN(c0S_-R+)zM{ z<+5cNH(^z$JfKzd4kXxNn?jzvw zVZpbk@tXGB@3E_s(={WLt9gnB@bm&cM8});@V|5ph$6S; zy^FIt*<>tL+jb>;FQUsMkLJoy%n3@uDPTI%;?w{rV_>728EV83uTL zcp)7}9&lfQ%1d1%xZcu?$&ZOQKs)R}ZMEWAzDA0_d-Mw9^m^A71VWJW6?TUyp#`bf z&TcZ_C%x*{Y|noE)j=N@L4{?_3!8dJd)YGcz@72~Whn`0=pzm#o#{9JweO2)-0o9h zq=IM?KnwaB=d<^I`<|eS`NlSbGM{=?^=-T{zq$oHbljGK@ffwP%5TjcX{;gsi`~HI z{r; zqW(9uSOSRsIso)<)Dl(T5W*+@a3pv`GnaDet#Lo^6Cxt2rjK6nQySzOIWw-*xYK2V z*2u+jae2-wEP~hht8w=V&VB8r(nXNSJ`pqwg~kILg##MP7mhi6Rfx)L7NHtXaeD>I zK*U?kStTcxN8`i9+NnQ322&hKG~sKU_9~HcK@nX%MR>C8G^?Ti5H{ucV6pq|Gm%yW z;;|RLZ`Xro!#}Frbg!M$+9buNH{ql-9FE*WaG@T=2-}OtHv7=W5cNZJR~osP+Ble6 z&8T1B66>E_nkeF5$M72RGMGs8C()EhO6eGt)H3Q@LO%Q37jFld2}k>Pp)!9gtUjYo zGp)khm@%qL@vy3W>I^ehVta&ZFU_*^Fk&S!B`+rXv@jz~q(MA85pkc64XHW_-)t(-`j_@;{>IAI z`tJG_|3OI1QUjXfPEWA@2~pJlL_pH*b^-Gwt(tvLCp`W8u0 z4{rT2b@uSfK7d#9R)Yeu5sY}qt}9ep$>|%k@%pll0i|vf0A8kT7Sq)VlxQotY2kl( z&g^RsH*oddU0;&P{!HGh>TG%C#%94j%PLiG0upJInKb4#s*;FSr@BD5*uNci0Ca7%=eay zs8AxQ(>wG?BM-N-#s@fZGY=xCN#lx>|D9IeXXR-(PjBmNa60=q;Ng=k($~K|E+WdZCl4gcjc$wDV?6wWrHrI!GFkGy(@V6dl##2iW zAN&WVwAQSkJ~+hl>=0p7SJOe_HiTy;n2GAP#DLKit>tAx_c-bkY_ z`=w^k{{ETwA^84qG~2?lnnQOh5RHf5A2|w+Ov5eYrKi)^zHnNQ>Ack<%21VN((U4E>1zh}pJWU8_p50S@Nlf+%gD__q2=($L zUA|+}5x0_yL*g^8c?Km#e!N8EJ|(d`^9Jsx12Y6g6Ng5D#~Z?=I5w%dlJNwj{wmU- zkS*w2kSvG4>!npN~#L0c?sA=1?m*3f{(SgTL)rcnuK>F--!nDdQV;_AfK` zyb;bY#xMQ4fjK+@pEPdU>0t^KmiE185Ejb427Iv-CaOd{3yq}kDgYM1xc^@>xV)^z z^`Ud{Hsi0q9BJOh!23*vvxEJ&=lji^p4t%hoDNH7W$)A`j{>AsW2jaEMmW~}7cQ}p zc8b-ErzdcfMIn8igD3uG*E@$vJ!aILVs=0BZV*cIN&-qALLxJrZ-$~)?0WMCHLe-5 z4Rv+phhj;o7^#R8R>42P!@%Fq0;j35cWS$uJt+>d%Be?x(I+)mCk6>WVxj%@W|M=d zFhfM`QPOza4U8dypPHnv;o%NSst=@Ym~7?9zUSF7fv?4BkJb@0{!Pz35(&1VdURu1 zdGIj+xu^?#OgPwb@;NHJ1h=doCn=%(JRVbw5KHZS-LPz4&V1Q~?djJohQyl=R_Nx`;U#WxLvfn|wS(}cs$%e=L zTW=$FzFE=hm0TwtW{hj{*L!iKDR@RKqccI|H+5jZx=eU@TiZw#GPaxcag!ZH@KD*y z{smvM&)*0(zufm!zdnaL`q^nHan$D;dTr)Q-H9&=al^&J*&m)h_{rebBg+R}^8lB^ ze2J>9_KHTANjXo`7Z@atAJAgwLC(8LB*7e19%Or^GOtV1Pf1@Fje>#vJO#rd2E|RI zSb!}y#LiJgH|b-D5&r~e$yGP(0c#OJ4X}%VB>wHpbg{obp1q-DWejE!F0y994^%2oS8!gHRO8M3t^Cm{Bp#lQ5k=u+< z^OTVm{MUf$uBH)*!|j_zixG@Z*gm0tM@tBVQJ1x@g7gmL-1a z`FG{*jEF)_gB0&}`=+kV;rpNjE683i?TJ6~K4jqJTq{sF)1_%>N`VMFJMj$0^_!Q| z90PbSaJCjdeW2crN!R~h2Kw9+J`T1Q7pqk|7neeX5X(@7{*te-k(CM(Xt#>y0eEIw z`5W&(onLn?bMIH_*0lqK#c1GunQa{y@HKV~CvdtIw^K{CI+fMhNX8HS{MgFS{>7I! z4=p81bIx0rl12PCllP$8ZMkyPYapGq=fA%cx7Cs3(PI5h7P=N2pBXI2kk> z4{UbbJTR*HJ4jpqgsQYE0w~urvK6Bq{Y}I&>0+ICiz*J}wa_PleOA998-nuM?*2=! z56y=HZVIDMn}3QcFHm0yi%ilW1BF`1`i zO;|fQu|=55FK45T|0}q8UpAJ5KNad~g*3N*xCTT`fNH&dHtLADEW|!B|wU zBm=q{j6hF*d^JU!OAT5|t-X9crv#Kr7?}Jmqj4Tz^N890w2Yo#s*O{`Wf6}aw6aP6 z5zWxCYxK&7^-YimoX5mPv0$y#Hi^I()SZ!{7Nl?GWw`r&NU+s);25>rvY%RD_G_(Y z=<9dn!l$Wb-SNLni7wgGAzUC|63rn3T}y*+e>}RO{-Ns=WiDKM72JIl$@AMr;!jwP)PyD;RJfW9TBHjEI2LqD=#a6RUn?lu{!hYnF-Ji|IwKO_%$~K{S!T9yaJ54gOTdwNkeJ_F@4p~XHRoBXxB=pbNp}a(bwAv zLN5RZt`AyL;)Q{oxvUa6Gh00FMH@zzmeE53dsQg5(YX=rolsggzubWu`Uu0+8!0#% zbyz=O$(9@5Y5Qs2H6ybQ)*c9wAJKyU^Zw1F=Hl+}lP>G})a()Z4}Y>GuY z|7iw!$xsFHOVC}ASR>xDex$nzHsfDMnC^P&B9K9Tf0x5q>Fo9Ugm#BvA!h+{ZP2G& zAWc;qy&}Ih^uB1b?fEpZMrHS2{9%LEpfAu5aAKEC`d%K?bL2RH+d z0O>Vj1~B$qI`^T_t~n~OtW0po0O=@-P{X?q`cYU9UZw-(w#88?pxKRj#i?{aKO|n( z5tKK^nY&`}fvzn<_7S7LQs!ZbctXIPsqvjhn}HS)5+$A(8>ZVpcU=O>~yVS8U#DjOKDyd?PZ@)H`CM4+53z`WG$|nw=!$_=X=h-}P8s3yEpcghb z`PksPP2AB)mDW(cBQZk)ebFOI(bakK_EumYzI8LM-@d0G35?LGcu|>v)_?F3aB$P> zjR%=dK1eVE>HS8z5VM=Z;e8h;`TJQUzvtlS!327}#6B;0l$$Q9re-d#Dkd&82!fW6 zsr;9e6o=NFpKKTPw^WOBc*&M)HD%(EBu)I#07rmFiz zYuqDSZ*P8u{Kb8%YuBQPSee^c+-VqDGTK0$7M0ss8b60925!9tw~3u-TyD;-VET93 zSG8vPv(P>>BUDh+q~72n9!Fri-;a5fgKiXW>7+BDjXbml@o7Xoqq&+-o#6#T$L4Em zq+v&6ir3}PPr%||Xn(KY_>qC^jBMTF7?mTQo5z^=q7jI}IeF+xsLAr}vaS_BaM&6pU8_#t7?I*6+hhj^3-+|?y zDzhgA7o@Wjsr>DU_yAmq0tJeCf%GGoaStvdt9+UON(E_mas4=P^H?HQ>d>g*(g-(e zNy_OXt~F?88eDlSRv&j!UDWk$-{&)q?o>iX@>G&$Le1mc<`b}9%Y33(86PB0`{->L zVB0ChqRKoi+x4sCTy^_+Alla`J@$qhn3tiY5IzwTp8KQU%ZAXbk?rW~gHrWVt+APd zxUtghka%|IamZUB|f}bH{tRGfj#yyo2LAka>Yb3^*3@!@TOHlOV zVL_dfA}PkH`4R=#I4eO$Fei<0LM?gD^RL{3dJp8joWcP7bDxZsnll6(eO=m2GZ`#8 zJz0zngiiXW0p5fsO+PpA-B2xwF`uzz80C_>E({+XZ!`i^GF)LvKX`x6p4PIy2q4Z7 z|NE~LLiI22&f`PKx25}#r_k7TGUR;IHSGLF%&vkch7U>d13lIDFU1jG)8w&3L&CR! zUkJpxc2|%}7x478Lv}*Uu)q(f_a{}PTR}I2pzeEXE9U9AZB1*a(uYd|gFJ0mp1gEtU^^S9;G+K+Jvq{wOZc ze0g)x>_%Z4P*>P?oU)B8qg^xT(`??(ge}rbIfEHv2s|?W{X-XSEa~UrFPuXqe=R6~ zM-ocLG`{?4@^|{b5^U0gKc>qZq<;$}qCR+V5=G+IX=X#io>^D|&fpe+I{ps;EkM%0 z66gM%Gr)Xn^dFOTrN1*zRGXD&9%r-iyAS<5k6ijDWZ9hk`kf=0LO8rC_u*Kdn9GMh(nKFX;Vuj`XP$hPU}22lL!|fL*=r$Eb*sVX=6N-HLbp<=RPw zB1e_Ye3AsHZI45ZDV5-?9VPKB$%vD@z{Dv2w1fPDhUNTs#sn;jzFF2360>0I?gT1uzp%qU9hDbO zhHZHN^oGrU^oB)3)J;E9&A-==3D;?T^AoJ)wO~B?_2Y3`jXOXaPS3dWz?&B`)z<}o z$@U;iYCSOenR__r;>8EroLCbnrp~tVvEo6OkYth<$ig=k96iXpU%VNWjip!ygk}*G ze7($ke4jWr>acUz5NI@rCa_#RU-?mpXDr+>v1u;wAgTvA@v_ky(?fve^U9tH%&77h z4ITJ?-+_$9no!#ZB3|uHnz?)?oZuUhQx8A^jz{=DVa+f+e+x(okQL&EMd9Xy6ZLA6 zq=7(x=WhgeQ1JmdEp$_74N^#w%`Lgy_dO+Gr8W`PYG_&w7;r3T%kV6$a-v9_05$1F z_l}CPGKf;90qX8*$GF=S1XPv6%^giPccmx_JMW)9I#x4Hqv7^>fa3Y1opb-kMU9BU zQzDs(yqjd2?nLk=AWk*5bUC*KaBf-@W=|iY9y&ts<^djcjqw7T`@U@Lum!xJ=U35S zkvX~eePX0)V4zx6=T&b#nIU})B7*pL)_|?{2XQv|8%j^IaSfieHl(rn?2h-e}^UTLNE1EEeBv70; z#IWSi8}=4;V5*v5XJ%=HvLt@`1-Kb4uSmSsYexjN{2)NRtf9MtnHvS~I+%k+%?Bu0 zTl0AY(g+6gU66TF7Q6!JuNKY0tuL3M|O+CS$h`gOA7d>?%R^1OScfi2} zo1WW?>|!2oAGBosbZhK`ca<^Y3w`O?c?O)P2H-9+FIc7efzWe~S0QAWoL*T*_>d{UOWuGg!5i0*x(XuyP0p;9#cb-EACY30~|YK{$l{qSTm zD#+_qbCUVf9GDId8{WIK&;SHM%hO*gH!%3OEe~(W0R;BG$&0xFYE+^PA;P=>?~|15 z96<x7o>0m_<>R;W&=C9XhZqn=hvMrL@lj*yq~C%JwN%=D)M;mtgs( zY00Malnv7~<*TkS$blE1re}010Gb$@W|?IvXvkLDMfJdcJ!ErE12lQe9SKsATl@Z8 z;NL0-Mh?Sq>#2_47mA|i1D;~XxiMTPh8ldRsfT9|>F!bRsdK?thvlnrUq1}w%~%;i z<*UPfUO^A(aceWpC3z($aDRygURoa8J7S{`?Q6gx90aDn{rjJcrEVqk)$1zJiVaTX z(qUrLJ#_XK%>9$2@MB(LexwnH ztdpiQC01Yl6M^?jF~nj=`7lW$kj8yc#X*YTGS5_!Z)g$0$P3bN0LU7_Pf#SY{kK2a zX=3IS^G~lFHiK1H0iJg3mkqFSN3A-@W^GmVz5{0BPghkbhgv{xoQcKuK>cuYdpZid zN+IL-6Z@xDvFweu<$YkeJEwpeDTgIgaR5EBmH@RlkK?(lR2z26#~;hdsnk2mh3cgr zfbaXXZ4`&yBnCXS=HAlzb>Hwaka|kOd1H;gv+j7ksJSuhDx6?E7#79Q-3lZiDZl&- zB1!UgY|k-tT~+C@d$5AC@O<6BKC&E{P>CN6>NrI<$pirG^V&C8&CI8+>jF25{+!tl z)!>Dn?ByU~Ui|HcN8@P@>k`D2oV)l>!(~-%CV}qc$(|za)GRAXCxgJ|AMnFO=on?* zn$Pj00}tZ!d^RKfdDmeYdW|^|8*KT_7ghv`Z+_$?N~1BRKRLB#^7y49X3q)utrtLe z6W9rYvl{DSyza-*)QJvF(e(2H`d-_!l*~%;%!eNEw=FplAC*&MXtZTz&dTZh=kNKC znn=`MQ^$Q`HQ**2e%{tF`F5EwD*yPI*C46xdmb@0P16Zz@JvfK+~Cnn z;x}e}K^Au6ZnTCYjl2&l>-Bmuub1swZ%fDN<5OBg_n&+Fo<>dG4~sT zcWQb#FqK$1Narctq_1n6fcjiy(jaD>F!5Li^dBcA2~@X?aA|A%3m5g5hbl)jmgA zYFarkk3NX{l2IK7CQA!^Q5!OJGo0}vXquPdNbPN!j{{GV_rzS7D)Bp)h*USGDcs&O z%kblvz^(Iy(Lq_BJaD+@O(JU&`+?Y0mB084-$r$DJSYm-f4xHk)LE&cdA5r;s%w{c z*|>Gw_#!hR&9{R1ANPn!Mubm1a!pSOM{9oph{SMJ%`EYoY1fwP)@oIXpYP)O{&=9Y zkXM%j?SLZZy|_Q$z!^c{1I%d>_V0)S^=KgZVB2C(ot%$T>weh1ntwiLXG)ilH2=Q48mxm9jExhuwuU*s^>Q`tC= z(-=OV&8z(#ZaM|a2aW?k-@|dH_pmNERIk&d^Q`l?4&`_uU+=>Q^fyDx z8Q#xi5HziwRNuRfK;PPkcV z!~E?Pxi4`0)?7ZGGn{`0mMbk9FD3<=^<0uL@=MHR{__To_rqUO4Eu)krw8rq@7&YN zX`=I^2N7#q2)_fPn|8~M=%QIyu2AyetaqW$p0Rv&(4k-mQyq^f0h zJj1|ARQEI@+%YR*PEQ6|V9DW{spf8Ts}P949OCPt!&q8A90&SjC+3^i`Lc9r-04*g zLmk9fhFv92#2`Ein)M5?TsAAp_y6K)Qc9{xn~u3ppVg$Xm!Fjwxwqm`x|U|ak{`X- zhg*fV+6CRzi^}j|T39slGLYqmUfeIPt2wM%Lilv>k~DN@ducS|rFk=SG#3mvupAF( z-ss>)D&*sP_md2)bPv>&LZ)Mon|DtZzP`X8ZwO!<%;=%oEAS?d2WIF+Xd6fjlUJ|g zJ(U-l-tF|Hy>aYR+{shvGnX#J~E7rXjW03o4Fr|Nc%TDmRgSx1VK6j zI_NY#m9Ptdp&tHxJOVfc!9y@O`nvUN|K`Gcn6!l^2e1i4GQUYxlw@=Pk4)J7Cjk6K z|pXwtOtcR<;qkdg+W_vfaIiV`CY_6Wz(!~*LSf}}95u-j?OSSl;uUz(Lo-FF-fX9rHfx2OID(1 zcCc!MQOb7=3LXry)yag5f!ed;T-W<9i~frZg_$D{zyua}a(H8^Mkt-Xu|D`cG8{8P z+qp#L^AH`uoI^kBi|#2k&ekw3Ew=_EapX} zixjIm#A!l6Gg(R9i>wjw+*l5M#h^!lJ;%z{m3uWsqauC23URbT@#_-jv;6fSO^Xn0 z8Y&Hd6@OCnB0npkEL{cZ2vW|~BI=j(USl2)Ns9h}LRpX1+;`f-#%Mz{!qA?cw{M*_ z1ORAsQCrmRh8n}&B`39XcGQhx<9yp56?rXohByiEqXMR(EQa|hA}|?T*TVyMK_nBE z91c6nISUjl=mt4io)t)!E}J?}kQBkSI49C*NZOv3%%uC;tS|jbeyT=RyEM4^6(#Z#8*`{MvtbFDr^H zI}5b=h|W(F<`3+l=sN^2lkeN|7sJ{P$tFMed9r_O<;8+@hpmHssm$QYuK~o!%D?a* znSOAqKpXy1kSOsrY2c4X!qIFKBAXvc6cIbmoVhQrTcvFu(ff$NAJ(3pe4pdz{I*K$ zYa$N9q0&KUK~V)iV~)9q6RLBtDvq8|w>GH}ik*((aTXJ@AHjoRkV4Z+yaJ=Lb~uBw z59RyO8jGstV_f9}EsnL{=7R8KAt3u30BYy*9CzH$1PFIkDtb@sqIrb$&Ya!I&B*J0 zZ1j2AXhcBRgEOYa{jLK0Cbs)-+CjpKkj%2_6TZx`oS&qE!k!r9d7gE3={-doMy>8; zHQLYTy6{kC=n^EhwRmJcE%ES_cnTQyi;r%>#^mtiGx0uZDIZ8Mq-_KY!B4w(>x{pB z55=KSXokjG(Pd!iORj^49wsu|QJU`IA6p#aE#f8K_&bQc67B@ULZ>259sFuph( zSbGd(o8?Dq7=>ldF!@QrWtxyw=K4xm3|a-%MQUiYilw6zq!C1Vc`+y#z{JM3?HrzD zfReb;KuDl zP-=}QlpblUB;73mdohbQ zNKiOZC(dly7_}nC=p*byZD<-N5j-~bG%5~uf3naS7`wv30rf3*JtB)gom)naE#*Z6 z{Q^lr=={`S+5jfX9!lR==2>s~0FeZw5a?7`mZ{sMfB3c&!59Xi(Ke5&m2$_=-}%Uf z>is%W>NdkUxqy8?oF^TrIWpStg3}DR1RNtnzSya48T10r00KEFnyAb-Y zJ#fN(_50d+@F6u5_iZ@3F61_1Y43TxKW zpjrZx^2qiEmN3wj(cpQfj}b-N$&hY=$3fkIH^wSa^~GY}6zLhX2juGlCSQG`$+=1D zrTJJVije;Wp`F1+?p+#du5g&eVQro7)EEHd1MDS=mg1RU`7i<-Q5-84tl8W9(+Q99 z_0tK16JLw>`*}pQC=Ac&8+-b=VE1k5b_5;__a&CX{ym>Dm>cB?jzAZmji%Uu*jh~b zkIze48@0W2yQW~nDIM|W$=X1+9Jz7JNIxA)@=;ag*k3Xg&NLrPB>pL`G0pS?+X2YV zuOS@mcmX3W^hku%XE{7)?=yI^JDulwJ7U}$j%BZonyJJUX6~z$p!b>S3qj`xxxH`s z#Wx|SEy2+wq;Szl!I2{2n5Gm)E}z8=yV(ozUOZ`NK3pke8>bNV7Q@7-Yf1Q8-8s`K zLAg)jdqZPC&nHn7?#E3MHB_2yrPKLU-?+t(#EIy`D~ zni_4E?{o>0k39AIq>n@cgo*AeAaxRdno2*;&Ecg!m_RO^$)YoN2(J6ec;^OcMZSUK z`1?#0g#qC^0c5*)x)o+XmBN0 zatppE+*E%Z1_uCa_S+MSVs3)!j0DAsrTvjjWz2a8;xFs|7~pOxa6S>?me z@;$0FW&v*~;07=H<|c!n+h|yq0;b-_ysQ7tZb{SN3*kMi#h^gLLW~>69*2cCxA!je zNdDq2++f@Y5rh3ZGeTfo*OQ2&cfV!Dw{|8Usxp%9ssk}e$e&iE*{th&g5#+j{jqae zLe-Gvfe$B2m*w4Efm14?yK<^-5D)bQA0U-@7sW&|ZCO!Y#W-W~i!OfpfiK*uXuJws z;PE7Z5rDdkznY8|vlYO1KzSMbz0l$wiMJnz$ZT=WEIb*ip;s1C2aKD5s~`$AdH8=~ zChR=gBhj(a2PIa6p6>fbZVN7lN(IShh=^F*?NUfyh|&4i_??@cbeZ?Z9Yv^fpuaGX3M$%mv&_RW$hI+7JT2SmT$ z4?QMe37xggJJuc02b8eR?dE*A+ z$`y#z0kMP<0}an~|NMZ`vUO{J#&a=VH+EX5O(OMIB z4o1wgki3&uDUOPUx9{m9Kn2dt54ac62bSP{c|y5+V!)<~_p4+_0)rWq$2JkgsXdUV zQjOw)Qo&(P?&726@!|rVv6eW{J2;SsxqdrH zPd8QR=HU?HVh`bmiON9W+$xYHoT%La9s-Ik>8^fRJ~ee*%4mq2X?&WN#Yo;i&nAX} zkB70KsHom!HC^FL1Ktl1oEH+wKp96QlI6skmH-nEs||qD;)INlEs*l_(+?{v55g}U z*Q76+PQ%gM|1_lnsr_zP*oO;er|f~bBaqC_B>3*Q?W|nyYntaoU4Y;{5R>INop9u; zyQq`Q2IPWyg~M?W-C8g8^5k~#BVTq#NG1rU{M{wNg_r-e-yWsWDg0e*V84%ofRV%_ z62;;q-n)8C1XA#N%4hKX)0Gw|Dy5(cG6#u{l}Q+@|5Q>EhznAh^w;wr=*8kW0+}4S zW+nj6qpX!fCM?jaBY~CtZC8$1j6SHWf9lz0*1$IS+uGVCP6>FDeF7_=O0oW!Z}XFm z@W@~D#WJ$ld|CgW7T%RR>+Q9fj>cQQ=sEzZO`C9Q)Q4-2Cvb=VRqfIs(t?tSV{1jwGRv$lEC%p(;OgK|xc;TA%Q7^XasWx*$Y@s>inFwzu8PmSK&vA;ye0f8!nq9V!FP2zvWR_?XRe5i%%uean*x! zFZn(XaD&Xms0i~Mj3DAX8;ksu=VXFFP<85uWO$Rif1W}q!mwW~DiY=c`mE4-+#2FP zekniD6_6A!L&ICx0>lwRK#lx4H9Q{VsGtAOH53_66yR9#PLdEh2fG}@5p7vNDSz?O zbZxucO0M$tA`Q`P=2_+=T!-aKVgCANL^26=EtVPaKRVjg z#>mWo{lR1^GRBE;;3da@ax>xkA9NOfI{TnEghuVZln2`l(cSSL-kIlUI1%gf-VQ5z z21oEb!%?4{BsVOQ;X^MM%WTZ^m!=u7$XOhrj=FA<%{=I>t5%Bs?M?gZgNVZi5gW+@ z#KJdv&;t7NI@%B8XEE}_&M6IbZb+ipP?*_=7{3H1DGKZy#gUULhPovByLE2)-6sV5 z$Npx~^ECt@0A#%&ft1QU~p8MiV`>wFQKBVcBNAmnBxh6w~}NK2U~*66p1xGfTEOzQw1 zKxA@HVB@&Jf^HaDJm5vy0mR>3aq~|b3k zf)!(QJ!`z6Bqf~KI7k;|Tm~P6I;yg}NI%f;tbt0ul;Q5Wczu-<_^@R<=_)H5s|!sm z+ABzvlL{lZWP)LreXw+TA_fu&s^uL%N^(z~Q*2_H=Nkv>7{;$}_mma9hR)=2NlbbnfIsR&L-5t{fT@J z=cKp&-Ye0A#PAj^E{tEbIkO3i{h((0dLN#nwynII8D-6o%D2OE-%h^FzhmR7mAuW8 zp9kHNRwXP4UEyl-A(z>wjE6Vx~en&R=r$4^TyOg+C(|_PAKuqYi$R3!(=U?Wxxc5u-|rXR?5|`e`8Po9 z&Y(Re5n-b>VJ1{=uwBS7Ek4RD1avH(?)xWU+l=L+(BY{?xixAQ8uEHRSE+X%;H!C( zIR~NNUk_m43dY`k-2TgJhqrtj5X|T>$F0}vUXnfcqt$|Y7~P#z%$CAi6ewW~)+z4H zjPRci0`@MB*?9PKqPvy5V?0qh%m3CH-qLMhjiKC`k*d%5z%PSrRjjLTZARtwI*0xK+L5NTfO#mLMRDGV44x#6=BDR$+7{zRgWtM0@cYqsU-6(Hs6ew?dmmB zy$cvaM!0(ta_;#(PE5TM3Bx)g%iu{jt<4k)fo=mCcmHI_S=NaicfPGmh@$73Fndh| zN5Q!mL5&hz{;SOQ@Cmhf1a?h!POee%GVuv7CFVu|Vg{V9$V$Rmw7q|t?1EPj*YT1p zGZS>~kfvyFI=i?Gw4>NUy%mbJe8A!1%=&u8#}6g@H zoNnYE{CEFe*PwkRw4KXPynjI#Vt%9V{(5HfjMwAIM{*~zFxUyBsl;QG90pF1Z`KP_s8VZeUx9D+9Lpo$n+utL21>LT$IH{ z_YFtM;Fi&#%$5aOV2*HYA9+4&(VYx40Ozxye$9YFqi?Np#3^NlT7E$+;#=?1e`DzZ z8eo8-4n64<0TV|YjVVZwp|byU*Nx=}?{5G`>G_>)90Kl^2g!|#8F~45g7%ir{i_S>gdgVc-vnuOx9ox=bpIr&riEg@z0CF&;uAdU&lp^OqdoCGeWKvLDtN{xm7yCIa`yVmQ`& zjn;IZ9-}IN_OXYtxjX|}2BPo$woe?1BMxmC11XU{s$fA^(LEza>F%HbzYNj14JEKS|(v%)=XHBytlq;$bQ z8XU6_06=dnv9|TkkNh}hDr=8P49tr&Vy!oLVhIFHua@rq9&rbZ17YW_l914D(%FsN zs7qDpEMVts-KRgPLNLYRLnrVwX;~LZBACuaHNbc__8MQazz=uupFPyaCFcLLSLGo| zp8ljI$9lHs_w@B?nqw{Il_3M%|M(e7_D|O(H$vD!X5}6+2Ja)sN}Q3^`uHk8-<}(J z<`)zEC~s8zZRhE!i1W!jHMQPfXdB2nB;-=5W^f|KCS zw{-L-O!b*AW(~wUQBN>V8yccH@OMw2CueF;>*p_ckM3JdqV77AVOmy7^a8GbUglJ& z*JOMJf7{F}n)kMFR%7pV{_A;hYV_18qZ1fG6%={5{LI3`I}erDFosaCThgnjlo=!JidkUkUN83{Ya3*msD? zHtO&1P7x(yjb*8#_fH4w?I(|*L~Z@eMO%2aH3UNQ?q)QZ^0ayX-NF2T)zYhbx+@=tZC@*K6Z;lh1VDR$p_5gT!&YIvPA`|5zNBAO^XJRFVyX%o zSfy*u8ys8bgCqz1aut3uiK6}a;2}w%N^rKHq2`16pwM<_ws{c;}3`3P!1> zZ~@|!7BpXs56R?1-Bi)JN`n_@9^P1|QbzZTQu#n<=@~xbsro)R z2#HB>&kivDVM<7TfHq=3c`iQqHsS30ueaWP-sX1m+ON^P@{gtwri%E~XZe#zyNe}+=v!;dEo*!ct15e%+glh^ zqI(Kx3Uwc%@Eh@2<46P9+)h=;<~S0El@t+(X5nfdT?P7CQ#;UHmRppHV>U>uiE;f#ce(Vo(+x%o~e!X`k!-Pz;%vwB3CE)wY zsy-aOI!tPDY)^LuNURQbNBqCM!g|l3rOs88c{$OGsdKd0f3Kn;V?w5BZ>pwkB zu@qa2O{LutBH0f)=K3UNSn!4~gTLu_eq6Z8C44RgD2m_?j2emI`D68Bem7nP zdloYaFqcsr&%LDj_4@wwgAXcs-arLUE{|sckOqx>KkpRnr@p0fLAC8X5Na<;68!qR z_m{|3v3OTM3)mR8RW5>j19_^Ur@*myKV4!r5o=`z!iwd}osUe8&uVu<7rz`IjYm56dPXoZz-#$KH4r z<8beZ5HZrCfakSQO{O=3R5TvLuoA%7)FWc*bz-zv23hY!$J_qB=4LiKVMoXfQOn!+ zfhNuijVa}QKmS&CTIitv(s@e>=|ZqQ{rXXbMBnlNvf{#6Go;NC07qYI7CyNP8nU;w z<-JiY<~hwEadz?4-197@r7^H(JD2F0Wr*~<7h{f%nYhQ3>a5-m8oaUE+p6LG(5k?R zld68FX!gD}F_(~cDCD1~;3_wPZvu38?UVLAA0SXNtC{I97M`NQ{q=Z#+{0QHe0Zj{ z2PzTfkeVpN1wT|R^PD@)FTd&Mp_Zqde9E{3YYE%nA3FSk&flK#J$K1LAn69)u=|i5 zj&;k?dQ&kXo;K1q{9RKU@|v!Pc&U&z%@nv3*B4C+52C_!_q(eyI0crJKW+6SV6#0M z5B~9NrtsZoO#8l+*HeElwN|<(^CI7FyIgO&3*?SddN-f?fO#`-{f|!#wY?Ep|MOn_ zfB099r5i(B)P>jU%|9q6(NJ(RCFfZqujBoI#wU$Se|M%<tpZQ~^T#p>JA>8JDgy*A5Hjjf~l^ZNDk9=I@St7dd^HrF38ybXdaxh}pPe^c7+ z34qODHO2-<==Fy~xV=WoFWAN$GJ zYl3Jy)dT(FYdTt6tD1-?o$9Ya4#m9tO+`d=ITyYmusoAw0C0q$5cv)H*Yjogq5)o& z6D?BLfdMG$9!YA(_Vb_KF4~2iHN#y)wl#-C6^PI{S;zN4@eUt8A{}Vy`dpmiS6@yG z?2zJX09RxT^EdzbbHv8}!cdYV<2)}6<+CUqPB~tCz2ETp=EY~_MSTvTv?FTx#e}uc zPc}{@xVb&F;b43^=)>HI;g`T}$~Tgb zX(Yg}+PJO>ZwB~gz+xf4tOpRzJm?5AsvH-yMy7|kGcw1_83$Dde6b4>W^wf@`h{^NlrzJSAUo{A0dpKtKe|jr}2i_KOUKGp3Ab5HN1iA0Ju3yJ@;G+S- z0Drh(oyT_Y`hR&Ko}YjpuV=N9&sTL&pz2pzyDS#?9K2%$^|{wg60$73duY7-_;h#T;`mn7Qt;LnqE_$^UsI zmUID9{7t{nFi1@RGr;FPK~%^#b6R0Q@b;>~?(?i%FfZayaR>ka5X)a*j~nmZ7j zgB*tmWmMn#K>x0P^8Flu4mtC~jSC&Ch# z>O&VUXrUpaQd?I}Mo+R{3W! zoLHJ~80~5l#%$X_BKcw99}}GsloSbFpxJB7NBSGYD}th#f`GME_p}6->Vxf>#P((= z=J|;>ck7D*2V(HzhnNBSS=%YilxEcH^Ll{mi>B`*Vp+g#s_uI)W2JGtfB&>mvZ}WK z&Wt5IeK~;ov(sHD8@nfq^-&OV(_$#Pk4SGRM$o!v_%BV{pEXXIUQuahi++Ayl1BmU z!1-bUnXm6MCXpFHk?<#}YHBov{wx$NS&plY{z+~S;-uJDThBi&)2kc_BORg3k0jsu zl$8?lbXH5FM+EncJu7=lAS+bsKEan`2l6M_lxx=tX1PO4a{}T#Xyua$+A(JUmteF? z1(bvFt~H$Wi-q-x{>){CLHKwl&Rl&DF8tK7Ry%nJjM6uoyr8|Pib&uy9F(HVsU28g z*wWe1Ut<0ZU__120dgkhanAyFT*F@A$ayC_7s8q)FP96{eu?T=5=avYQtGPny%$jR z6e!tdw_x8ti+?w;+649+pjC&I>n{+sk^WGHC>GJB!rswjMIeuva`Qmy5puz_AG`|d zs2pshejR|jc+hrps)Mthy?&;vudzte5+9MeU2H-$=GGU`unTMCrT@7Q=zn||=ZEuJ zzaBiTpV2!eCxWUl&_E^C9 zz>h21;r_8aW9&w|{XqNFlMliAkii4ByEwPS)FH@qGxI2Knr4#0;$bfcntXNJo*+4S zf<8BRKQ9uQMX$sdfqf))uc@vP5%UI#ijPp8Nbk*f+S~Whg%27YS6~{>o>ko<*Os|L zQFE69&J%93WVBu<%q=h4N(6&-r2u3pR0u)9`Ki_Ij6QtQL-MPOfs4dkz)mIyuQnq{ z1dF!)5&~tdPoi4brQ%|X_q#8C9TKyTNX*(D1Wnl=uq3wwT=aPcfWP~ChiPUcf!ghf z+AqgAX6Kpc_AUqzRCn;!i3oSsqN?Oipx2=EgqnZh1#lm@DE=?@-mK|WY-tnwD;<4{ zKQm8ubwx0WNeB>=xMLRcjDTP7rFYkwWlww5c&dJMp8-uEVepZuYZ z3S#@W38m}zl|;xrAa&^D+{FG4?Osa5aF%V04r}huoUdArJ^szv&;~1wiv=j(zUAI{ zPL@30z_7?#2ic}??AOpE@c`() zJ5esZDiX`_)p6WsYeY3^M{k8L&HQx_9?J$PW>$6&fQzOsrGLel{CfN$lJvkn)SM*r z5UgGW=?0d)Sm&TBAg={F%~UtXl$UAHPuTHO0|hWiyuzceBZxo_1}4f8gBtBS1anN zFz$i-!&al;`SovX^JV>sFXZ1+Os^zPl)UntHlYasQ*;V&JQpQ?J{F^9&n;YM2RZfK z$=A}V3>*LE0SU0118ipp-FjDh01TfG)XY+31|9JsI0kN;vZrX39B}KbGRkFEQI0z?B(!UqXm1NT7gY7y)Ma zn{Lr*q;Jl@TL|>BFb80y;dtmr6)cUfY{QZm4^lQ|G6qWq|GLd>IX1B5D=jN$I~tZT zJD)G{z5^5vP$BH|;XuO}{LQ;qIy>fWqOg$*=-gJS5E~T}SWmw>J~7|^#;t5rhchpl zl`-t&wxn~+ylb&QbG}1m-?Z}?*!~<9Bxu`AdI&vyjl?HoZc^*P&g`o;z_!AbeQ-6IXGZDje}hhY#i!d|>QS*+d>KqZa?ImJ6NvWsjXYRfQ7 z(=T+bE+01TkTgwSPW+7nsKB1Iz~40y?vpvFe?H;1yY@aT5+7h@7%NNqE>ka`IN$1x0YMju~tc2U=?l5z|5j$~U`GaU>;uM?^h3*W*l`-{`uA`FmcBiOEj z)thJ5unP_t3>__4X6Pk}cVC(Pi8(G1x{?_;)FLb!!7?NhG4+5a6ZgGueDpqhFiaD? zKY(md`n!Vp0F@1;0Ql0_zqu!u^t7T||9Rgg+zJY3cK)#82j07pTV=NLHWNb{uiK#B z0Tb7_bnw8i$_2=1fu2ne1GY{I2lV4*z&GKYu|wK-Y_5KqnmwEE>iNHM~rX!@YM)YXsFqA%@ zJOOt<*Yv_iMwiyl1{nr(5aN44bZa}CAT0#lUEc?-JD-z`M%(F5VT$$+grtg_UyV%yvpR%-=f1wVP?Ql~BbV3#`yK+oW7Uv&LWngnaj4}GrMv9T zZWHR%sdQ)>$vmVXQg!MKZ{j`$u;H$W@b=pQRWk>RQ-ZBYps<# zYO@Uca!MbKWAAoURJFt!E!P=S5(Kij>~(bT!;8_VR4HKDzjKo>hxJnyQ{&dHJ8&Mb zd5LNwkNW6rlD_=W3dxOz+OQwTmYpHOJSkHP*PFnpjb36OR%#}o06b(>vLab98y)Z8 z+#TGj`>yf4LLwxpp^x)^4(KAw0`#SNhs^W8@?&s1EX=}ll3#k}jUfPl44$B5a9MxF z@^qpBCaqeJ&+(!n67m4;b)T%ev1}mFiN@!-tCv$McUk_g*km zgKtkS{r16|`{ZA#3>SXd_Z7P`PIzE6J+_5rhYF&3SDs=hBTpnfx_6PLt(p72 z_x{aeH}Ca^KcDSIh3Ouk4mBREKLl_Ot)>8%y3GCs_UZJ=R-0LtkYN z0PY=HaOhr|fy=CBJO)odPu`00T|+PMlwH^^M4^n$dbir1Yf!PVf1hHd*83+=Rxf?) zPny4Heze4IKo(IPg`kRw?^)QX=xvoZSXi^boYWx*xA!9_;rF&vgF)e>b&Eep=z9$D zp86iE25feXK8#-R8nPVE`}bYAJ1^Qw5Worir@ZSx3FOQ%d?E>PtI;9dei@hj8w1%T za5C zjii2j9>c%+Jfe?#!obE~K987{E6I&`D)W7ojn96*sl90buz4T6iid<6YyREXsKTV? z?TTLL=N<^5{X`vKehCXIe%?WG-lj**3jxs30ClWuiM}*_d=uWY+G^=Oar!rBl~LZJ z<{KaCpoOQjjgl__68#N=_j*;0Xw;Z2xEmWSeIitnUk3I);O4ojQzukhMfJ?r*XQVc zZr?mZdQ=Beqy5AXy&qr9dw%8rbhtq8#Mk7cL*T?8g6L^9F)#Q8rOt)K{!l$>TW_t5xh9M<4KyO$P9Ni20%T$yZuv!(M3$F?a8io*5&{x)GDw zpu6yWUDpCwVRpPhlhKZJY(dGADAx>q=g0X{`XkrDD9NwwOnnq!+c0`${)vS#P?n)a z(ugW)kb+fu#$>nXCuXn<&wU|>Me&DobjT{Z9E?6f?n<^z#%C>ICGFrYC}UfPnlrQf zZ4YNO!|}fDr~F=PdIQAFF|@H9TtGk)?Ka3uz9Hkqurf2^`gs?4=T>SkBp_DUye{g| zCO{%on~TYswrj6`Ze9=c2H~pTm>R_7N6Mr|6OsB{M;Ydm$Nih`X7Nj@j{!-Ri)}*- zH61qe;xHUnHr6I7(|76xZK_BFxl2a(V zQp|rBZ&&6q&2~>GqxL`fTlk|>`#=6q*t=Y<*-ASZ2B0!KbHbyh3ARjeHHWY7Swb8Q zVn`nDcuAzp$d#jPnA&Ki)GjL0S8^ij)8WK7&ncPX)J{jXuOZsL>y6*vg~=BP`}1Cr zd<^9KvH0;C@F^nwB$hJ9juL7@xn3`G#D;40Umtp8;afj#cE6uDTC04(ntQ-mNHRJum!X1G@tU+O<__Ge1xu<+OAr`hLgW#Pt zv7NH&%iePrTEy~H6fuQSTR49cc8)e*Sgt~}(G8n8HqD$IHCV^;I#$hFDBd6GL16G5 zrK`%C9*?+HnrusJ&%>2PQ=eb^u3xvfbp)QTG~1+%*ybu4?d(NhcrczH{WaSYA3oPt zd=l!XvM1#wp{r_qy-Wb6f5=i*Ved8#o`a+F3oos~Da9CM34`pbENNmxe~zGdxdwI@f6rwSbByC0VvpIVt1B=0__1jip7H>;Zp`gyt(opzqDZ4u6U=80(;u| z1;fAgaJy1ijS+@JFp6%G zSBuB>lc2lln^&horMR&zwtlJ-#l1)h0>p3m`T~k?gj(+m0McDDQ81N(F}SD%>TIw1 zxHB7^-RxbE>uI!IXd_j`zsc~$1(cT0VNX*cEPq(`0s1g0ctlz!eszv?|KegnR2I^E zeS#isx9i5a_P{ei&yrs4Z4c~1tD5XP7xdqa7Sr@`6JMTM>G)sqw%IX%C_Rb+6Tn(F zR5m_2&EoL%JMo4*K8TimC|7f$a%GoRCgBWi9jXHAqkFJLN6)RdTZa64`7Jk|u9D!ET& zezbV66Qe^`AcM}FEk!SFc0By#RyKQHw7+p`IZd1z)sa@4%wPPbmOR7%%z2deg8)N7 zyuY^@XVT#Cr`--SsgUp9DXG7vUu(SDx4eOOCk2`Nv{L~ALo>gaooUBkF`?N&|790e z4y)}94T1e=ySAaKdRuoI_F4sv6PFr|VtSj-m07^-YF3k`Jwy6oGM$g9SwugdIH0ef zN_5EE2z0CeG>9(gp66}3yIP^5y67zkdap5NtHCi|}GGTHo>|2EVET)_4#S2d`t zWMTQly#0^7n}Wo*o*}&e))6>m`p%IE^6MSH2HXGUK97h=GZN}~e0@`+KYdfLEA`+z ztTZnJ*4gnfk9y?6fd6L&D+^l4_F~iF(&Dv{UOb;uvN-5x7`+R|Ez%=L{U_F>R~1P< z=0C9}RZ=p&=#Irhmamk1Vc>iIxYf?!={rh}@{x0#prL>yjO=+L&S4a(2)g~ycU@c_Jp5(ZLmk$H!-|JD?zmZ4+g(u*nBM0`qxhLtf2YL1pfC6B`Zlpf90Np zNn5`hTsh^@RuTn1BEAeBF%-7neULz5If@VdznSpDM+P>+bi^zagiDy)*oLXN7AD@C z{oOurD(O32y>5fK#-K%K6-i7wS@RM$6Dl&6CoA=yGNn)#bsK&hQ9T2JnSdgi=zN2U z0K^zPd@?+o?vsg-tM4#b*d zs)cL|@I6W$oAZSn-eh$=(5Lud7|6#V zDFik=v@*@VZ`y%Sp*=_E_R7eP4kC`PTM{aqa$g$t;O>hM3CN7n3eaBI9?ZeCKK#d8bHJwI zyCQsZUX{1D5S+?^&9V>oA|!)f zKjw^H=5*g5bi9j+vE~mApv~K_JiKiN3_8lyEKAsH;oVqzDC@=2N1p z?k;uFn!{7l^hpZ4N$*C8V`qN)#AyFo)#jED-Cz?{HhNgea)8=r*-50HbcU1!0Z_=0 zk?)!Z>SN@)rkFdlXOT_cM_r~Kxg=|SHPDmfn-71Rd_cOp5`m}m8{N1p`nB~kXrt8| zYhA3?jP4OubRM_22z18N_InlsCEB{L-wXxtma2DQh*M-&^KZL}Y}*?Y(V3cWzOf#< z;BSoYvrr+1`CMK^g`~{^YyMI3miNb_^?UqqWxm`sg8tbLM<0%p<-O#M#B|*sZ{E~` z8<5w}C1PVfNFQ&C?k@rW6$uV^i&AG%gBYd$GxyzxZLJUFkDqBZNr%Ih**!4&zTNa7 z%WJRmBecwToX5Ra-z_W$(2CNulI=U5eIfYWRLzI3>vmqdfU6+uC}}{5wj2XevtbCr zJ_EzwPAtnv28zee>#qEY3oDvk?aeBM!0EreWu)D}4 zmmNRUH;1(NYP=u4Bu>dVk&+)@0c!hAV2hBl&r)d52XQUS8{pgpRxX&%aBb!T`L^Wn zhi|u+Sn_3EmG8UPBC=z@xq_*n zO^xo7BO4cG+s;!N33d6*+|Jof(|n5XOrY1Jj@Yds@g4>I4Y!~^;y*5)t~dnE)K21A z=7&%e(TM%_Yh8WZuIT>FQKP@>af+v@`MRgQTgA5&yRYBJ(23qc>6P6jR^MYSUWj%G%jo7(-GON1_`^!M()wM%pP6sQ%rS6 zfwh5wZi7+)8XC~V2?(ue&czjWIC5v|x9Ndic%5}uVWh>Xe(j5X>Rrbf()~4JXS{mT z$Gw~sDjtuJgbu162k9n~_sUVR;X%cb*`3t8c5_H@?%gBp^19q0C#WRG zz3*m2N{mjTkwlLd)?g4^3oeAL#PPahO&HSWPQ8J5cW-8$f=ZfJBug6A@8L07F=7Q{ zV)5krt>}ulZbA{PhNO?m>sDfapRksVU2L|Scjo^(l1(s zQWNja3vIX;%iOlkb9y&095&CP#WqCM$!GzLq$cK!{67{rw@E&mMqv(-)(8bjk`2r1U?x2(TN5}m`{li34&(21x0Y{O&O zYi%2%FXHKE&la}o`huq0J?}mY0(sYRVu3;_xnDk82}|6@W3=dgpF=;nFZyf!EV;>fKu)D&=DZisT;fWmYzUOY;d zq8M%OlTfV2Pb2feo5QLqLP9f^F3Sf=KQ5z9V9-V}U#L2pDj27>EkWcHI!o5hVRzKp z-t_Ri$}>$bPbMYI!x_K_6=XvfGF&@3i?DGH;$)}kJ_-RtfWnnz={4(MKS~8id1cl~ zzh-ReOkN=19DQyNOF&JrCf4k^KBj)_0RB2}PlXCo`1%lg!r!o^!;`h^!q+UJ?|2By zg$_NvjNgwLp)%7iiMy?c{?rP07S20(nYHJ$S&aqGg*WL^2n}FFo>dO-^WAluq%Bfa zlh(gJc(WHJFi%4GFdiNPObGdHTlVH(;zZQXDlrUk@~>A#Ps*7|`vclhfLPxjA`@?@ zWWL@2o(2waOO+>?R3>V4=5*7&c-^ohXa#h9!c>u#Dnat_BlpZTcbDX`<36jO{*WAeykx?pI7d~QjPZzz_qA-b@W()j+O>SDhO4Og6?!Qd?3Zj1MgzeOh*r1Z>+A$S++QeX$1hCbZeq?d&I_;3qCfG*zN z_IalZGSNtQW_z}{#Lz5(Z6OG^mh?{vSD?U(g8JyEvG@5M*pl^(HP3n8^+K-iH(V_^^a1wHA!h{5>?Df~fq&VPn$A z%aUdL!ySRNL+p(`qXl6eMJN|=g>@th6;I1}+NzG)2QX)$(Y9LPPf@VltQpOKS)_K0 z4#7KQ8759d<96e<1P|wdK-Tn_@cZbjaN-WK7AqtS6Rb2qRB%f$k*Z>cjVF1k1zKd6 z0*{o#03E2HKuwA`Y8^hUh;2qaQF|{o2!aA6@NK=C?-nLXeh84V+?>2{0~mH_fIYl_ z-u@!__3e_Yw>d&`a5GgvhY-0tUPE~AYTu}`ln`q$hNE20^})WZ@wJB|8q^+@Jxcow z`;@b|&-y0$lgY&WyRfqxqIf}owfJ#C*smZNkvJQtBrXt0J4I-^rvU22)?gRNcEe{Y zdYj4gScAH+SqN3Kgps1vYpLCZt==jSN*>|4s!q0INM<{41YD>SHoGi9I)Mdi7o#vX z1U7js_q>dBeEPL9xf5~Zx~UD8oqlGLZ;YjdNQOV#__Xixi)$ zF6)CqcRB1xOR8T$`^T4vI%(!rcr(CZ4uzrgo^P=H7w{Di$KxnjJ@v3b>3p)>%OiQ-y zGc{w)*vzE%-u@2@+$pBb$_cG@%ixgeds%#>AtGkVZ5 zgT-$%I3L3(dJ~md<*c~&O{xeW)BwGPUZ6kIGGxy|ec^b4KDvE>NLXCBTnM?U>!Us` z`^!O$+1y+hRq&qMTk4*T`&PrFc-@Dq4?YcKrKlNAgwQ&dgp#uxGI@F@Bnf6q`>PepF9{YEV_(9QK=4i`gtl4Bv z`|?=<-<3Rys3!ImTA*Rqp2kz{jyjIN%TJ1TQFUElXP2yLwX^45tjfbPeXQGudcN;I z-W(`stj#B|$iN=OYM?&P(D@^?X7gkq7K8A7TJ}ezK_hE`r}1T&~7Xb1Ra7-8^y>odwZRN$$Isc}nHK0g}4$>VKq&)X;) zpLaY3(X2;;TULZHCUC7E_d*Vr-5uhOt>of@llkA zzN7S&JUAStp%4e$(Kb(#z{6>4+)lsip1;+$Z~9QW?ulRlzzCIa4?%3}pNuKI*L#K% zNNkeoB!LWy_FGf z6w7y1_;!Yo#Q_0T6m-{^_iq3dhAntiMO8j#81QH_s_sL}nMVa-sMIqo4<*d=RC!BR zxkuiR(d`P%pwev7fhwq7AO|jLqTJZm_T8EaL7~8e7_@c)>Q>n2y~L-zg{-QS?tpUU zRSSWlsf};>lQUyke3V>s%Q2{EH9%Ej6JE~VEm|6W(!tffQ!8-aM~W4@C9+vhpdOp; ziiVum!==eE=GFa(@sA3;Z?*h1_M@^F^J?quJus2V!q+ZdExwb z3xy>eSf|kN*B6+)BRGyaNzc8=>{c%I{guzho2s(6iR?&OsTk=>+q-La%38*U2LP2- zz}*z*`E%&(fm2L}Akrx?0=1?VYyL>dJw_86n3>H2?b)!_@&G8MR)D=oilS;boCN@G zKz%=3cQ}wg;|K#X#(G;gIbxAz+IqV~bDS8BWK+qN$V$&}HB*9k&a?K#Eg z;##d`k?-!(DhCC1a7$W8k)1MHjG83Kf@-t5Jv?Qxp&y$_O$=`0{kmrP9ncl_x?p%W zoE>MW1O9x{hb*FxOZX@>C#GJ#Lhc^1fK!Q72_+}4{7WyR@@g5MK86pu68flr9WXHv z?2Bfx4kB`BrL;Z39ySJT1t7zWh6}qr!}K>crZG1#WgYJ~Ab{U%k8_yx)&V5rvBQ@@ zRaGMctHC`v3*C2YHU;wiQJCVsWQ6|m0rqz4Q!urP>X9oK+ZJUWM$RGSR3z_s4v(h` zxIPpzyK$~YR*kR|48g2_F@WK_O8v$XWTCaS*u|2!C|MQig?Qh}k-v_MkL6M&3@A2W+q%otj4^opo)#d3Yk|G; zhm`?VZ6eqZbS6!9)1k^Q)B^16Ly&^$?{Hk*!a78QAs+*HcSqz1T?e4cTY*>*JmWOP z48A78iJGZpAfU>@4GzKNZn_s|Lfu4X>jXc*p(Z1h_XmL2^Ci?vuqs&g0C3Qa-_-0f zm^;0l0f}Qz@%7{gCd`r@1x~1l)&PqPkq3>{twio}JO@mJV?@>;=QA%(I z(?!}-_><$F*i6doKKq8H?RX`>%v3kZN|)X1(EWIN437GZso?N+k=GnR2G>x3tR;vt zgV&LQbMd3ZRjBN5F^n8jscvio-`%Zs_>Sy!A#>reUl6<@uouLv4j#Y&^JhjMJ6C0G z)uR}!OL%fn@&WAq9>dJ*1ST9?bj76ux{te!;}a1ATri*BKp>4lRoxG*s~)x{N~ zr8jE%$fHFUCK=#T9ed#H;GdHEQU~nS!So)rP{-&Y(?iI`n#T&+ zMN?aDRTX(l_w5cdikjy^bisH)Wzde1jH9%WiNOrogl9s#MMGnG;Q=G?5n+{JJUELA zDxg+%ZiKm!w3<_{dqX{#@Vy~*}c_x-g&9w{x}4F2R97S9MYZDK408lmC*T* z;0uF1k$Vg{eZz5o9^|jNMJ9VQv-|mbopkw0fJ-6IKAH7z^>I{HXEYBm;I-IO9scH5 zf{i=FFzV&cXlsZ4wc>78j=Nlyb3)%_1QA`hc-BaesoSZyde%Pi{eWU$W&4k(j^(FFlxTJ znzlXAVUVi9=Af=BtR8(D1WX;F@m(wMCM10F-MeGT4vE?9p*2{n34nZR-+ktVUcV9A zu`(Y9%t<5J`~H^6P`c}=dthMf`8eM9O57GZPE9XJreHrIzI!2-{y}oNLrOzsmkLeL z8Ah|D4pmyAzEt^UNTBf1o~s$r-*Lo+J=}f-HbuhmGp(&5;=|fTmOIqw!1^ zsg`fP;bz}BDk-W@iM!8$pSe~-{LE}$vi*&f07k4E<;a?Rn$b7VL)ehXy1NvcZ#_Aj zu{E<|XV3`JY!inFWnZ_KGl5U(R&vPh-+a%YLN8U`u*8xxgS`*+2HiU_j=z1X^cO*! zIpBp0ndOt`#cyFWXo{NFlcF$OA0&W5CpPbc(WCMQ)bA{`dlQ|=^>NrFa+anAGvPCR zR9!5&l|JoVHR&^%eB%z(C;{76*pBb_bCPT;z&x|5-wYhqBSs6=&$=75HUOtd9K~+< zeCRz*1N*+5*#pfooLWQUXsAqor)lc>%ocO9!$-8j~n5fug>1SbNnhsRJqKfU|?pKC(o!}Ik&dwt?NX3)t;O# z=v?s1Z=iq{VPM#QnzKAuq-w%99p|?If}k|M_;7g3e(L8)aubNn>N8=UfpYJx2H_W(gpG}~Gm*a2#>+YcdR{)Q*BmNE>h zeR+c{6p{wuNDG)U* z^rsg5d%{$^K2*JlO#Xw!nv_2j`Bo3wf+fi$IYjzUG6zwTvvkMibY2+7tR`%`UUVDp zHPnBn``1@(YF~*&3UX+^_z#il9C)tG@9|gx zG7i}yl<>KOL~OXShg7{RyJ`B83+|iRrjR1M6Ah?Bpq}~3_3pkA#Xz?O?u<^znOl7M z2w?~oJS^z$AF8LV!+&bYCthAWVORQNkIN&LA8@h~Og*_bOa!*(cDvQOo+|C^>5z88 zUtfk(C$o*Ud%JuG5G5j-Jytkgty1VP`IxrA$pyio4`eGdG%Kc_V&7mw(oc48Ir z{*;?ksGg~Pq^5Oakk#j{y=Y&b`tt_5tw5jDOXFx35Q_C{6>Bouf*hzeGl;K%|7f!K z|w)k^7Ai5!pz zk<=<I5De3X{LwS!pSAN^=Ow`*`_JOuSHI2^~ zTym$SR3<{C*$tc54o%lvq^YO(J?p<7oSMEWk z;r!3@+f9W@DkwYt4YKFLDD-QJ+nxML>Ns9dtB*m%QdXu>v%PP%$ry<0=SOCBwWDFP zGY;@8v~ci~*8x-XTB?H=e+bu`ku8{`U9=A>>exN^WUQThtpPGdLDZHVW-h?qQtYim zqhJC88hSYNwZKI(W9l3^1i)X0Bi;C-enqp<(!|?t>?DEE{QEUd(*5N1 zH>~;#uR6S5dJ@(IXgU7-HPHNRfc+bc{*F7(ehxBkj`I4_|Kr-XOlv9q|7H6BqD-n> zsipuREEAc*^ zzCVNfoc#qm_#QyJ(E}JeQE8yjQq&RUX8E~)d-{O4j0s$w*-xeTC;yG%^n|`Hec`{^ zF6-Xe9M{>lDY=8ptx)wtia2g=I>b206z}WW&6@zgxurMS=exm^_S`1MJ%2cnz&T$m zBtg>6A5_4NgqM&wGl~!Z`}`!o3)hwJU#%^Ld*_K zwAFr=&{!%@_ss?aDCiLrPIaa~eHZb;|4As*D*PmMYX=Vr1u@N{y)(0&ugawsWOl-%rz{r$ILuF9&9Tjy`5f6Duh7r^Ol9k*4C*ZB9}+N#h0pDT`$ z|5ToTY7Ok4S_Av1*1-O$HSm9G4gBw|0spPEzyHI{zkO@)`yS6#oV59KovXeC9sND| z+utkQ$6!F1zrjC$f7|1B%YR4ycN9TDS%3ZdZ5}?$@8I1(e}7AdcI*0|6aV`9zuJrh z`M+F|U&H#JD)8%yn*E*=C`smc`d=i;kpD$s8T(%`Nu~dVCpm)UMM{(3_x^eNZ-0qi z1|IZ(0ODWf@Yt9CXA&bd8m0R=ay(O;%-=^>HTP$!d@K;o1Opbpf zJ2amTqH^;>q?G+vAl6)_m%{GTaIqqIsUYD{9d(xE>iJTa%sm5da{LGO9wP?xGoQ(J zEebZQZ|@LPNb2Zh}#WMCk2eX zbdKJ9WG9(2{$9X);_V&sO%PjA+qN|$LamM__xPh1`|3LhQxg$60lIVOUEfzTh6QQC zBrpuoa78CYF|YOG0kMPocA8TtpsypMdM7CAyqpcs_Vj1-!E3^jFbu05A}$!RVG7GO zG)~hr%ZN%*aDrG_t;kGSCg!@X-}fX*p3Zf%09gqulZMGUspolK&+~c&F9%v)kS}Nb zMG5sqA5K?)Z#4ay^gc zBP>F-o2t@Nd4m@;xFAUq?tKr>v;E#wKf!}s(1bAKvbWwnpyU4{?mfC41-h)wAH)Ff zhIsEiGpz7l2>A8Q&9|z%x(46sS?3^EuFRCKAmZ%(Y!PvyvgLMN%5a(nl7YFF#cf6S zbztjNl_p>S5P26+Wx4cFbK83B-y+aG?`Zls#J^2z(k)}i_&r+}q%fu$S_|pd7J7m( z*1Lo*1T%i``}a$$jrUFW)+iJi@uz9x?Ccn!4NZS^MJ9W30Q&08)k^ z_j5~8MUun4as@B{4dR^MlP+I4&ZgX!Ikj4F+(32LPE|0b$DqXZ3Lv)KfFSqCE4x49pWK&gyG}{YX(mekRccToeAm5A7nXd1I z@$S%F$kcx87&5;;8Y0zMhfOPcbSqg!zIkXFw(qO%B-eCPc~C^gPm*LgN+GIx>+nkB zSQbU!t~!L6r}?u@33g+yQ@hE=EXRaU06>uMFO6rkXc^@-KA{hUzpBSS=LrHpY-tkW zWW>&1AIjocY6G^ApdypT-*NoBA>wjGUv&9IIm76klPqgz3XPd@#$Ek!UOk3^A<3Rl!p0ybPW z^hdI;s`X5&@9TiBmH36EPxdTsy3GT_Xjn@OYai1+JS6`0JcC44t;czqrnLqfG!cF4 zG&Fa>-U!RKRYfY5WR$1R$Ek(C&#b&z3ef)^7Vn}vkgx{>({~S|i0Y6J8joM$orC?C zOfoHF^Kj47c20Ne=tY0F1q#PqKdmyY>6*W0+E}q%Vg!?Jt%-7(Bt?1I6$qk8n*M!J z5qP@TqP@v4hb^!?SiVv_WMWgU%pOkj-zJ{H{Yre-eHc@MGq_wZ+_ddReucv_zVB}q z5zr|03K+7z7qNc@FU3}`V5t)&vPU(HVK^QUMUq?+jdI#kf z0Hrm=6fsnwF#fFq%m*NQZ_ltX`ihkM43DdOeqfHqvV1B|GlNk``qH^+ZKv8jF5d}ou5N^T-*tH`OL0I&yyicepg}M@AsTo; zH<*|dl-qfUUb_$FQ1uA9MLx{p6jhq=CO%-Mg0<}KcA3T><>j-vNNXznz z7f6+c1C{l92AY-BUh^w%Kl!BjcRK3!%@5;W&M$-?3Ie>+{q^-^!VN)ZGG21NC=+q7Nui<_}-~&f+?z8JAZ|sQzgsuPeg3xCX)S9L6N{1kUS!}Z&vex_ysq3n$WEemCb zNOtzhv`%X=Cr|Qa%r?iC3763Af%KfMH7VHJkCRwN_13l$aOCq$;|E?; zGQ;C7Bvk7_FBWZz6zxv}zkj~OAqL^@sP?2^eN{eK2+1cXd@m&gX!)!G9P7_P9nM&d~n%#VEb`6<^ub{OIj<>l?D~lMvFTCtjo02rnf!%uvGOcK%r@>N?40bek|;Xu=cZGiKib1UgO7;*mH|3( zTU8&3nu=slwZ`oG&qmT)1?oAY@}pzxH?1rcGc@;CU8P*K-PDD1 z5bA-=a(TDJL;e_p1jy@gsjO}B*?oO^=Ou?Hq#Mm&^{qwuc+G?5-)K!O~YIpaOPQd(_3&jIyidnZ#Q-p%FEpGG^0nKA~7I)dL_g#JjJ`4 zpzpCog+3mZ*&h5PoosWQS1XL8QXfeeISb0b+WqVH3WiQgtUjgZZUq1=YgvOJLP1Oi zF;ImzF!EO%^NP9hh!c$FwnAg!8|J{*zqWYugX-bP9pZ%u3BMF*>8wE(Qk;!&w?`5M zVN3Y33Hgk*5QfyDeR-SAY-gy+Tp`ghYoo)=!=}=Ss~zAZ?mUj>!=OAW*xK}%u4I_s zZRqk+bdwwJg1Y|vlreqIv>4%fx@dIPYXbxje2kzfhC+l21=jG{6S4zUrgSOv@H zp2vWXIN}XnR{`{uU{7n`&HTc0nz}0ahg_C#v539xBO}X;8oOvRlwD_S8F|q;{2Q4t zM|FO1HfcZWbEbx44O8)nHvu3mP1o_HvtDxXw5N$;NT#^I6W6s#oTSto+>hs66}kK^ zbXv3(NYa8X!26?uMmY12!uau;tUxGD`UJOf0nRfduR&vnC5Z5z9-hiNCrx4AjapV! zKLE9)%BCtBXMk>0MiN;xZ?}1%t{j74UBm-S1K;R+zAA<Ts0C-5vyNd_toX}$x% zIuldWB*b_YrAtGI>5vr7A#(s+2@G%(|~hpR@5`hbUkS$gCX- zNA^KGc}<0)OalE1U2XV&jkXMX)Y3a|ebb;0bEU1N_$NOoXx)8?BogGP)jr3+UP;f3 zYo@ewl;L|X;@^*0Oa1t+InYze#nq)VubVzgf~gSs2-hJpayjqekZxe~c)DeFzUa9KCRuSf{NeTk^K$`Vf7Xv_NWOLwT?!oL4^%&ucZkkYus z{B5om)_XtJ+<#2028IQCRPqHHJj~#r)(S8~iKJPl24&=84=7FSFz8Kbl3jFOtUr`6 zY8`=v{v3d-Bj^B9dFR8{Ge~22JdHTw@Dd3 zDTHhZG7~Mu^aN1>vmUuWJs2=k`>2{C)YnT|zK^lE7Rqp~w#x;5j|heDe*2XcM(!2! zzNiIe?&Z<`NItP*<{~sFp3=GNSo`JKN$QJbbb+-XCDJiia<2-0$O(Za_&W`Tj_fb3 zH+AOHv_d&Cp{>Y}%D;62XtysQSm`1ZaEI4|9f>=1)Lr_^2|oOe9HyH2#_tWxU!upF z73msxAsaLE1}n6*Q5Zs+%+TY*G&Gl>gp+Zo07IkqrN~TLOQH>h9R5tx{fURIcGPwc z1sn-}aAiD_F*Z-?sUJjJtl@+%Hne@hAHIFyb^faH#<;f8u8Y9~Wm&v-L|6YL-@9>d z0KN9w7PIGtLP&nY$F3oT>oMT z*ObXm<`-xcAGF;fY5m^iw6kJp+CrNhrcwd@@db1E&c#+cfj9~wG@w@+XZtTWjpKdz z_}Ms4`5hH1hW!X68e+;+yo;`=K61!Xv_mKK>A24~!QCkIcW<{WM}5jes_dQDaL^BA zxV==1mp<0H(q9A4re3h5sL{SvBCl$R~Zv;g`>(aP&Om zZ1M6tytB~>m&^Mlac~7B&idr!iGmm>kWiOK)G&4S1Dy4-GE2Z~xpM}62a*Q#P~_E! zZ_9fcDj$r`(=b}gD&5V)dPhecPv9IqkAL8|CcH5Y{;qo|`yWKgq)u&MUc5vYjjK?* zQuiu$MS(B~SmkxKmTjWdozV1q4R#3P-eCIrF}5Bxm~F;`7Ma+s55jV+_Vdv|)3S?p z!Eq6~>1ks4F**3NCeS76(o2Y_DO#1`9QJxCU0>8yFc471Rs`3+I)daVxDmlcQ?Df~ zY?}un4lFeaEnNltNq6Ad;7r$XPOJAeJ)$b$Q?vjJ{Bb3E0?|E1^zl*q8+qI<6yVKN z?e$fYV8ynor{1}?eyIf{VQW&{pe*U|LbvML0vru-cS6rkz3%7u>ld9iJ=k##x_MCp zZi8h^%@N5u70zVfJboKx(D*ougQHhtIZD^yxWGGDJyic<6NY*6w&fZ3gE}7~SP)$Z zZ4CSR407DP6;;;}?8Ae#m3r(-@}V+=gZ@B`e1o&jkKR0kXLuoP+JEj5N5Uc>@}Jw30hg2G*ekJ{6bE!m zg>OwyfA|9>se^?_)zlBHw#6)K9AIZMP?a*LIVkX(Q)VK0Bg}(*6 zbP-aAoUb6H9u@QT8rsSFj!#_4R+^yc~7t43Ig51Z!ScX104ubSy{vO z!MO2xy>i2%>6iz18`PiRS01cG#91 zl>HDw=m16~o7buPD2#j7nbmXY1Ud18$rIQD1irNebOi;0hf2dG<(e$<=&JfFGm7l= zU)y!{vY036RZ2O;UR}-L7bkcZuIHm$S_k9Rt6CQ5PD@)X^Q`q1;_G@OwrTFeu~1GA@PX?Xo?rYY+o#Q=UaP!F;01B$1ajGgNQjux74i zS9rp(m)}n*PL6kb`fP87`a4fCcO8$iHxXh#WdvIh(mC>9uV{fD%A2spEpM--rc!p^ z<8UX8-=|S1S2V5c%PVf^GUKL8qD^GauM>|3M_cc2xnvbtJO2JxjjpQ=ZR$gj-9}wF zBQDJWHspNOcY}##i<0+dAsT6^UhkavcqWYq=Jq#H7e=(?Zf^;12;%5gJP33P+<>N( z$L2?o&htadCse~~Mck8t7p10ahXsN8I6@G4wW(7Lc~b5X}X`&0|=|F3`EOALGk>nKisr#6DJ~k_Cpn7^ss;m-BYyi#BeezT(_s zdnuIit4G3lGb*4icGZom0tmv?eV}wg3uYOB1ef3Fve_9`Cs#%oG*GY zN?Q0PcBjaHpTrqVqUnHv=RCHJurCwH&_gctfc0BeLutXR##h8x%37Zef&^pWzn=~= zs(;UxsMa|q>k|8ZA~+q>BOD}{EdR0K_(6Zc;&ymaVKC;z-`9SauK>#;iTT&xcjB5C z=P#zjI!t{5Xr0rMS)+|E%BsE}G_DsK-Dbj7RrpTrAw6b-YQPAN=Fb8a=^XRpz}V?x z0kzF8d{6%%H(dXY(~Y>;;@$;MruB1kuvQmywu1x=1&K%rXNaHzy*5+D9&T z?iqsu+jiIc-YpZe>P!d4o07cW=RzII{N&D|-|DJu6MvjhVhUW{Um#R3zOqi`-(v9B zb21M@?%==H6cjzfNc%RwjEa!njuN!+Es4j!h=hJPvi#fRSG0W)V?Rcr808YQPvn}R zfHkVChP_TQ>OFaZTsJQ3$i7h#+Vg z^(kQ_BfnX83b|01D=~7z^C1a>voCl!PBw8FVD<8RTNm5)=6MAGY;7dUGb7mSq>faA{4lt;*#L{xnpP(v@-;5m9J?974*6`qmEr91` z5Q0#tKr68HFcBq6|C&D?v_nmmB^o7MNvvGnl>Q7c1(jAc2I=+djL~FtiNyU-ysk3?-ShpA-@m|C$-HF97V*cL#s2^ z?1^+Ug87juL!%rTW1{kdk@GCm?jj7L_*fe7_*9W%cibl9%rs|tws4J$RwQ1=k5v=E z(wE z7~1eO?qgUb4{&IJ2Q|NxC4PVxH|0|u$I|zxQZSv+y)xq~hXIkoPaHUqDa+_8<1h_? zFEk;kxgh-U*C69t!g5N=WqBOp{?I4dU5j@i4edeT(q+vjJL=_hW)q0~lm}ItR2%B} zJLD><@0oNDLf`nSo59uIqneZT@Wgj;mG|#`T^Ht!w@-#v&4d?N`oK~5=U>C%93+f) zzR0%iMtdDq29Lkxj67suj_xL7o}Yt9KtP~~s3Aa@T=Sd@3Zy0UR_}XXX$1rv|3s$E zM02nO@8_84PsiGNY$0~C?^8WIz_(te!H6a>r2jUJwSSxNh3(%uf*7Ezsjiq**>lJi zK)QK^kGjrF8*TF)y6K92foQL6`KHZ8a%ZsKU@^AO6QqHmnoN@h3KFDprpROrdeK}S zVgA4dp z(}7bxCjl1fmu%_+%mvwtrgrIiiS60|nWa=fw1zZ6z{4XrP_kQXB@TlP!D%J@ryZ`a zJ{jtpoefD9U}!6~lpi%JY$nix=CLd|+OtS(nB~KK%4%*pt-8Krx*EA?Jmr?A8*$;q z81KngEKJC#zq;G;1>f&<4G<=f^k*FVa+L7>Z}aBR+OAiR7|0l`6AvbC=wovaH8Ecu zwtO<13)n)uiBcPqUHUusVbG924Hw^e+rfRO_uupS*Xb~9!g7&2=utt7|gl?P+Fp(MCp@2hb`co?@f!WM8*FPQp{lJRUuB##=15A@H zdMj#sd>P4ZB+Jg@3=d-NklC~G7i5q~G}FEC(F+HFnmReQ zudY6US+7$RMGoflQ3W?=H&#q%&}|>65AYg%c=L0ZqR*Ml_oUV!k5l)VmMUE#hG9O8 z@Yjgd`FS&QR>({M#jhvTh*q=C*4BL&Xik{0viJn_ep7DElC<6 z+P`hw*CB!v=#A82D(RJjFe`>3`2I6k#Co_wy&bMA3SteY&B2>1>*(P4M0nC?*am#|XT zWd)yC0xXi1VBgn;K?M&U3++w+)W4j21+$r()k-`esuXlxxV5;+P3oLY!;8bdt|A$( zH|K1)G*lMJyncE$VBDb~O|e_(d>g&sQX^E1MmG}1uTg*X=OhRbI2(eYpz-k0zZxBU z3?vV<2>$^Hfxo34*TNg^ZF*`OdcH8c0PATwfW9b`&5PW@eaAL3)JJsrzXLo?cZgOq z8%?VaJkx1@t*}|r?w}QZD;I-$nv+g6AGRfKz+r^*S)UL}){R0VF6}ISlqaPWg?+i% z<%7iOa)x6)j(PRT85!%DJvHk%;c`Fo2r!_yEZxUn$~oBWT^iIFX6u{i&jAGxfS9B7 zM)eL|)R6?f3O*xeiFwXpcFeVj4fh3--L?PuCGJZ4J2DeEPZMNsCY%zop5K4ZAHpdX z^z(f4L*%n${&7yUxsCJ924IP3QnwM7ITSGj7$ZJt5S97N63~---B*1cS8|Z%^jx`Z z+oqxVq(@8C4ye?q9FJ`bBAmdUrcx_3CJPcCsv0xQ9O_K*g-2>a8R^$`7Flrpz6iX~=Sh-VOai9|lFs|)G&O?NDNq0W{H-;9^iw}63KBb`e+Q})dv(L( z`*y(kt>GH@VJ|-;v9yhE0sVAv*Nr+Q^GyUEzQlhYOWq(t+sFlnk+Hj+&-d{YhK~gB zr5RTEm=0GM-)XI(2aRe5%+KF1NDw_m$ms5Tn7q$yVlub%m}so&>V?}pv*WI!AvkkW zO!-!*%3SvN58?tY+g;;IFiFj(&&LP_^CB6CgRX$Xq|`tJ;nTdCteK4@fQ*ZCCNq{8 z14AU|eOaO@S=_=#Xc(7nIGjL-Tw!v{YiZ}j)TBg4sb^}5D~*Ga9~~sx!|y@P`wm4a6${t z7ig!`>q}!vt<&tsNhW*<_hP&H03;PnTc;+rsIUeA$!epc+cw)tg;1n1MUagR0u_N4 zDaUKuk1(RE7oX*tu2L><2N$I2Nx)AqIh3U29?;u@^D9L-hO_KArTA8ENUBnYULmBm zS{{VDJ8y)+i%L5vVe2)y@{P+=Y>o(Z#oL>0Eubu3HV-os)`4==WhMZ@hALRWbRXVn z@1V`v+cx8oIc!NCVR@wP$a522hgrqxk_3kI z2GV;i>lS@KMgD4-aD*tFc$6u?A?|LPk*QdluDt5fN#DQz3TU?Q(8=#_@4^kc*z3u9 z=+clACcolYK9@o-%B)atl!JK0b_}ms`7XRyo?rj4XAQh=9ea5VJ;^5*H0no z=6V?@!l>NAhkN|s#$PIRs`>ME#qMlbKJ?rl7_67jC^NbEpmq@X1tviJcJc10&&`| zD*=7ce`*XE?n-(|2lKBBCv#3^n;d=3ztC@?EB_ikWX12zkLMkl(x;O&<<*x!vJLb% z*dP$uhUGJy;qsrgre4iS&kr`NRNCc5_Y8w*3u9DOS}?7fI6fCggFx`bRe*DxALgd` zdyPjkjtD9oe_IXAVVZ6T0Kw-Wn8^O`v9N>x9t%BfwMG9v%%O2I0<9@*SD=RnH$jqG zDK14jQ9wtE!}0wt%;m-A4h`6Hgziy=xw&fD;oy6uhVSH!CUO|$qi8CX|DCNB*QIHi z{rvm)^RZLcb%Yk&UVQV&xf^Gnqw{z4Go)D}u9fEHgW9eUT6R08U)X5G5FW(g%~rL; zyr@`&U(&_sYHI0*`_vla38cNr-Q?|M8HWn;AbAeNZT1V;z^2bZr~RXC0^wIHTfCd3F3xfU2q73U|YU*4SPsO^i(WVtpE1ASmW>2 zcTo-vYis@v0Z}qBlDIaTtF#RLuHpFYKXp9zcz*?77)9C-Er7~T{3PJl5*LaJCf=wx zgadr&uStcd0TeaQ7>-}h+Th)MY}9Z5s@Z}DRXQr!0yEbh_N=sW4%51?>$6s9sUL%y zqvOju*LX0D1B@Uc7+`oD^!pz^RHYl4C429%RZ+Bli;IrIWl773_`dT9ezSM~kRRdY z3Ulj)_p%r3WsCoBp7m)X_njXo+MX}-V*B;7xbn9)_AJ)Ty9wN7|2ATX?JrS8?6{ui z3iYAARdSk)zxyQ(82#1@Z+MTuG=H4-;sV>U3F9RzlkyR3@J2DHzI)BeGE@Zx!Pv_2 ze53md1jHE}zm$Vwb>%C4roMDdwtMkrdE^KwvT3i6;=?ubXO92j#MjrSUkuU#x0#R( z*{U9V9Pw z?n}S_?SuMi>q=lAd*Rq@Ht&o;)?DC1jQl$*#q!l2)l!q11m}EliVaZSeA)cut+t(v z9;E@~6E9PMxYldk;TBUj3C8@oQa19u)xUjqE_fkq&tk!-e8S#G4<@pihXb%qVR-OL zJvYr@A&7SDo-~%LPZSpbNgQMt9wBn~4qV6RIAki{{K$6GZA+Mo7xvl$-z)g9QNt&J z{Ta&4dxZL-zSaNe4OOYZ|D*jaw{5GgS!)kEJBF`xtb{Uswf1fh>#*yzSUZRrobVu- zQZ)RvfcWQs>L24Ak$m`n8Ucluzph21C1v>L2|AeiUO`$?{+vIM1BLq#UQLOftODW4=X>cLcM3N46R??HRO1TFh23Fk^6t=^_kT^tI@-hL z?%(&}w`u^iQr7?3g zRgm86hP{|%x_I;vmT;pW)?UL3;nw=cUFwvEvsi!8q#4f(he8>-ywV&Y(m~OT*!V*M zNKm)oFn#lAXg+>br}+3*?)Uq*W_o>yeerrfh=MXK-zgUL$fnuv#9fFrbD3irg$qdj}TiUF|j%>5uU^+jx$@J*d%oWPi^j zwqgD029)s)-+b;xW)XSE+5C_i*Nae(X8czS&(kLcZan1%FxswmIrm3hynN=Wx|b(v zx26|h)E5nbO>^E*oICYi{rP}C+Bt1So%3S$1?%SIKjIhVTef`C8hLp`D8v2BMpe7! zz=ct@4qBBpMQfzfK%E7Jx`|{fqW)Fa^+wFkHP@Yjfrt~hVeHo?nSm6U0BZJ?&ZAVv zR_vIc&!K3X<+@}=^!#Pj@OiZZh)HB0*JIqq*F($iPxHfZG_zH-b|N(#6>fT7f#tUR zmTTYchj0W#nBvT1L>60ukQ^qV@dyqq8z!gotgAcj_?OE#;qQLVzlIMkLA46laM-7F zGNu!|Pcp2P9{d6am3NlW{l9~m)Ba^pkNeXK>2o!{^9U+ez(vA5U6L}7*u3+4d0+NF z^y@#oq$`|&K){CpzJ6Ej+3+|WSh{c+TuW>Z!;cqUOY5t;7Ieyc4T6TpxF+t77_7}1 zv70J`18$Mgs}6r7@Cn-9yI}bO4-LS&{{HVeJ&No9l|{Pt)lYjC?CLx3GqK8TvHL*m z7w0E+zMP_9n4{t_)E~{8yJ7LKamQxx6j0EhIFwZt1rYYuojV(M+rGJE5I-;EDgqpk z*LrtSTLY0$g|DvCN;~wKWB8#92!Pvv3KPe1Z0NcNLi&tvt*onUn`an6t15taVfW|5 zQRBFsEVz82b#|X?uIXMumwp(5f3GPuuuE>Dludx|ap38#$o8}}fJhu)bi;4W6h?)R zB35NXsZs5_s7IMLI)BmNX-7a(uAHAbl zZ=Tj|5yxpzZcy#NvnE`a|6_bV5!SKO@AFEyqYiIChq+NS+Xnfk#UC&8G{?X5zjOHd zL7GqysA3t%(*jjiDxmzB&|YrM$Q&8+`BjO*`&p|2>uoD&40P{J=4qayFug`zJoait!oe#=Jtj;KQcvU%e?l`>6B97>xSwd&Er{+0pWP^@BluaIyaUMy0;H?)H1OvTa#e zt!dXgUtEpJQFMzCy}gvF?fdG?%OR)_ct zfHXYB)8FpWsPBg2jVS>Aj{n@05xZSvlF-P6Zr$b(R}8}R;f-V25=8Uj!hrF)IBXL*V>5zy;jW5p{e6ycKvjKuXKR(SQl9sv4_$lclg+q0|ev5w$EXiHUN=c_!Odl2^A z-~P!r!Ktrbm#`$It+bYok&7wguAkShTFErI{Z74)OTp{s4(+~!v?bI~ISj)}Wqxn; zlWqtK(ol+2{F&l7_KML@m*5)|-MThBYLp5=yq?jnG4s~#lY~p139qp za%I!VisZcWt?T(oHSw1xa3($3Tq!KMaMKir2EId~fS=6nx93aFUfJQzK^nOsuN}e= zP|l^s5u^P|Yw*xBs%MAP2E}`G=Ucagu)q0iTQm(D4X9$?+NRx}`}@0{#+__IW^60B zjf5lpBQxJ2+G8Zd*KH+X?y~cz_BZkb-7j!}@95>#ZA|oY@v+WvIlPjeUto*<272M-k1(3-gK z4J_2R2hu67OYI0*$bMV^DQ(zAK@}xRRm{HIGTbA*uE)Fs;H?8Auy)dWh2{AbUDIQK zY^x#%z(K)zb<6AVZVW!na8ICM1I$}RU$lBtt$NVf0B-P#ir*>T)_wg9Fc?#&ex@deBp*+Fysm9Y1ZO>)UH^3S(_eqr43Ga*YNK2!0VURMLDtxfgQ1N@O>?)jCasVcDvNRKWD zjT0F({rUtn&wU3^;je)rnrrV&uEEQy*ADsXnjBMOEHpSp83LE4)RSWP8;hrD9d#WB zsRYn$>g5l7p)btQ9MH5W2ZUL>xnN{8da-}-So-;Bt@;Zd}{G z+IZ5<*-pX`#$eZ$9B){F-2O192B9|#%INy*u7 z*5qV^0KS93MF$i#f0o>Svt@YDZ??35+I_)ena$WIVg}4U*i;ckPQ7<^LY!e>XyHwG zK>wzcxQw9hnLyRq@Ms5*ltvb;E@iTQK@An&NZ%#Zr-xMx2Xs(tTDeqy{D zncUF-m5E($%yQt4WEf%W2!R0Rh79)eOBr~ZOcv^ ziy6grf5sg}Ufsb4R>N-pRhDtl=&sJaBxP3+bCbngkR<6k__Lz@)2KOf z&_AAH!1+7LpZ=V;UH}^}P?NwmZO$RPbDm0ivzM(`4t$nyQOs zbki@1FZ(<6=DeA>m?wM=XSb1 zX|`g1hTkb1;c4N&OVzuFCHh5Co8kD{fF`Vu07ItCzo1_Tl!@08X{28|uRWL`%!YT< z%OJKyp{ZwDuBSmRCx=^o-JJPVa52GMow7U^BkF}TY3>3YxZdMfKF|O;tMam0Z?8CB zl~!FcjB327U0{7XL_Qhug@4rgs=ti)$^!w~fIjUTIs$QHr><$r4YtGYf+zz%PwD51%Iam$=q`s!Qt{Cs)b zX%aG>>@a}JUK|B5S2n5ck^$@B4Psf8N;o9(0}4%pcW32r0Qp0}&LH6Uh(N$lIu;90 z!{0T1=sC;~7hG&JoUiL@(BhBq6_V3(==+ZMlb?)x9X9=b(0iTy+p^#3y&=x*|HcdG zzv*ED9hLmhPNI9&d4VVHxK9Hf(#6~56iqKV^J@u=rS}h!7hM!q?v z83KXU2U|?Bdc?`pz(;ML=$H7pitW>9J1l#X*no{y#)7# z=8W&*<6jS26p-qUL0ZF#cRlx+u>jp-pjUo7f~C_xokb%wNJINHLQ43L3C`9enJof483(Qv3gBj;KtNP1QWbxQutS6JNjMgotR5&X z%LuH1`zW`*e6w!cXr(GS0otr&+6_AO^u`DGot{5;0kPjzCJh>p7>li*^ z^UPB>4$_vWFN_>6GK2g4lx}Cra`_WNJYy!@d#yeKX@gG}Q1<>dCy~}v(3OR7OciSv zS%K$3Oduxq(_hcWhgt;1YHNwI3&T^a5IR zO_}F>sD7P(DKP;`>EovI-+J3@2oi~Pj`X`@*i`4f5n*-}js^i#Sub~!g=UPx8@Bs% zL4J*U!6Sy4d zq8ALI+v+k?_{!*&y#Q0jrTHLSPj=t<6JQ$~MU>L#T9uGtHz?A}+TZ(qIe;I{?`v3*g2j13Z64IBzlf*`IwwE#Y{u2p|Uk7(y4ipNCT5vx-oz2i6j0+$9F3AkX;9} z{#{)?|L@vq7Cn14y>2hP+O*u!-|9NIe|~~yt^wKYqX8NQo1g3O*+Fc~hR+*lfSqKM zuuH3#&8+{Qb!_=76C_I2a?~BK!ToveQ&O5d0gTXQO>ytzXdSP4oTEA4-!%}e?Us!uHD-!4QL=a*25w zMUH*5dnDY)TC$nxsK9O*qDQu``I%1EDmoBWfY_Eoe?jtjZ#n@LW=HeHr;-M=*W#Ul zRK1n~ngiTh%J_>1RuPP~H9a2YHkggsYhU&=v_p@+lfOE*G^5nef1@wI4DT_OzvY03 z3A`RVd$dtKFiqwSZvXaL6_6f{32oYnP=%%|K;E^SPTLLMuk2m(jubDZu!#q+I53|go%K~q`#PT=bYkKISdm8?9~fuLv_p&IR{3{Asmj%IXx$u z{zamaEGxT33|?o`*cE_y?Lv5>-AtDJ*$d4rFrQ zRYija>1D9LQtScHJzRl&1VHya7Vw*^B^!&!Nhq>mUoAP zy?`UjGBCWbedp7wWf)u><4iTDmi51HTS4}5Lw8eoQPjP|p>$NW5HPji}=U%f4u3>?S85mQKY zX64iwEj@Ux4L&wQH|^8Y)`+&145yX;)vg6mdsJr_pUU)E^BrFQ`Y50#P!YUR|LrB1 z|7$PdAvfu&e7x3kK*Etnu4{OGcGR=(z9IQ6y1O=_4&{&dr>pM6VjddD-#R!>?WgzP zzwYy1JGVPo2k~4^pD*Fy?ppalzeq*~@khIB<$c?yEuI4(=728iALez>xvJv1H0Z(H zwGriK$OkX~)zUv6r0E*AaPQ?k7T&BM8TkL)Nh76tj% zh5Xk2$~&}eVicOgdDok1*$*LPFZNV@q!iuihZoW`_^!X3SI)~Y1pO}N-SqwCoH&)% zKuteA*~sTt4nR{hlJXfCSJ>ux$aSBL9L%34R)_r6wN^rCs`EBHY&De}O27}Y6C^C* z9SgjEYkWxzDMUi3=(l>QodCS2>^_(m@CAGK)g;f{sI3M%uW1Uy?M&08zGE314A|eM zI~X5S2S&wz>Otze9wglZ$UytTz1L<`tyQx_1kVi&%)5UPtD4~|-#Rrd4e@W^tfIEX zwT1BZjUo}aATB=)E9(0OK|vptX+-^j`Am)ZdU7vKJfTp@=xLfjx&k>%ox)P@0=0GD zpSR?R`v4SJQJVcbr1D~I#7X*FJJ+Z(LyI=UVP)U8u8UWIAwl4=X@+#t41hA!F1}Q( zDGTp?i)6PgvbgdMNR!mPf|6WWk8i!bwfj?JqZuW5QhJ`G-ihd!IT09JjQEy#+^4F z@#Hbpd&&r(Ur27IHg?6RJlu}MNwVq*GlUI<$FTnF@A@Nemqp@mtgL7L4pU|PD8heV zYp01A+u_H~+Vy-NtT(e1TO9**_I+PhRXWr6;U;1T&|K|+n$#`U8!vJ>Rq1ct|Frba z{Q*9-{JWo#peGz2N|dGPESyH~jlrZ~L5u!ZXH76=E5UQGW#!wSj`x;nY6&aP3&^tQ zZVf7II)AGIpGztC`x&p=kqHGeCJg9`PJ3^XboOsFGCp^|clMl1&gZ2;uzqrZ!u3HQ zw}L<>(&N{QF|>~hBsYyhP5D2yK%B$bz52W8gtP#Um=TdSKp(-2k#J4drxx@Z+$Ozx#B4AUOT48Fi)k1v$$3`+3Fp z3ITFd{FkE$?o96*F#lm|Zp(DTHqGDU{px0A)aem$4oKo?D~Vu^0zD$XsIiK4l)HP#CaK0{k^q*X}p zGxpH3ipfqrGQkc0Q)?Jd>c9L@4Cl{M!Wk{=Ji(i(oyxN!-Hvf=5A~+e0X)SHqspL> zZcejeYU*Dd*f!5X!-@6XOP5>trlVpzY(u2_c&)nk{;6sut_qTZoQR|j5V7H9-`$i? z)OlRTCByjWnwL3q&tu-9<$R&?fdGyc^qptdZKE)Ph~sP&KF9p^=b-s=gp!x#+RT`<*5VV@`MQQ2P@A z2_1iIfG0pQKlJL&ft2yM_qD&`L(@5d29l%VO;g~DvedNr_NnJ&ie~8cr|KXqkumi z%O(%|L1lhd(6SnC-bIKxx#DDuV{>16u18Hr_Kd4$ccx7{+&x^3{TIi_p zPtK4XlhfY`s^HY>j&}DwuH9sc(h~3p+(JoXD-8vY`LTT8&p~}t*}hd(#dzRe*jx`K z8c%EqaMf~3I?x6j@uz(Th9sGfoCvSGzAZ%M(Go~(h=-w+2|Kpmo)0hYvOF*7uJfuY z@VrEoVRTs|cFVFrkX{;7dT#0*!PsP4^DrP04DYCT5JXPq@QQS7@hR6rj z1zDEso=jW!E?=1@d7m8-Rb*{*tz!&7-hw4TApNtOGL{O{lq#A3o7+VvtbC%%VW27d z1}7ottKNHN+X8gSceOQCk;M0=kPB(FgTdnk_r%iJOIc(4a!j`eUPfbnHP!UdPiaoH z9kN1^)*c(`1{g+7uN56kQ|*-H?$y=iilvECsL~A%9{U%f4$WaBUm69AqP|psx4Gq1 z!6d7GoD7-vL*(xTQ!vF!XaMq_C7yDQ%D%@ z`(r|3R;>*VO$9Oba1<4FkvBCIMd5u1%4{XZbiG?h>v`5BiRaziH~KmO*C8v+^r(YJ z{L6YU5O=|ZAV$V#jON={{r!z7NmioOgL%t@&6ybSq)nmS6Q56doSvdKuZ;TkfloUi zT><26qri`R1PB#3uF^q^GMEM$IGLK={bQ2^Z87!>n{(xdOfxHh_^Ae zM!oMFK{ti4c)#Op07B04tgLNOST!;q8-~FNNRVPAj2i_i(WlUrl92L!?LZMQ$RQcIZ^n)k_U16}GGMi;4a*Q8F1R&ua$ZKX z!3cHOF{va;bn0Sgz%#3V^&Ne8Si0Zby|-f)c<=p&ocddoFJt37R}6dlq9C8+OjKkC zDbA?Y^OcJ)fZPqSY5-}I0VSq445wXe_Q6!BO&tb2>iI8X(Guhbs6AfZc~09d3Zl%` z?8|)~3NZ#z^p*NvGR(m5O%noPBD`W<=&S3thSkL8bmY-_huP#R!E0Qk1{2 z^GlUZV>PPxfw%5V_uZ>*Mfm-AK8Y_)&p>d0Y%nShTsBZ2`rsPpSCzV8W_fC4_;_o= z@{4Ydo|b#xywT3ivb|jMbNei<9$gI{52i%eHh(9Mp!gC05}ooG1olRRGB<-jxWZ8! zBN1)Ohcg{Ng><;m*x7X3YMeW-!S!4TwanY{UmcTRhoT@RiUoawQ6Ud9I^rPFAA;&yyJ8 zJ={yYU1?nzOd+LjCbqE!V;)RKU{^XTochB6X_xvF3e~#bhEmZH3=3UvdRE=xlVHSS zrnUDE>$oZ+T@L^;LS5HCnd%Bk>YIfDpTE9n@GSFOGy&`eYD!+8n3bC{EUzD&(O13i z+AEshyo1axR2Bl8Ig7189j*&%$Rb-c4BJUGyK>z7XcC0PF6|%?m6h2W>R1mB8KOE+ zl!dJ%Cbu}R1_Hs(*6rtdw0Wop@0drO{HxyF#&#GCz&y)3?fV;o0*-$cX4V$+h6WVA zOPa#d9uMIR-Wc~7>giK$_a2waH9akUgwW%1%6xoj*aL|SR*aM*dH9+W_jCCkdZll} z{V)ujxn#(CsGu1zn`qt0Lu*^n$B%y)rX(2b0J7`55F3551+P-H0uv2A${6V2FoEY7 zgO6GQmaX2A)QW64IMc}ZJw)(ry%&jkMdFmD5ByWYeF48tz7(wDA@;FpQYr`gYFrJp zuj!xenZUa7a_19apVfKCth&E!t>b6(`~K%|V@;v)w&B=3C*>R@45VO%J3Xw@TxAaM z?f`EC1-bbpN7U73^D9L?94jWDy)ggsa(btNWth(lYS@mx{>*;*LgIhr`#`ek7wb}R zAyl^hh`XoZA%ZXzpM%`9sD6YHFA2P8%kFsXBgy%DD~`+F4Ar#b6r1SMtK$(mCu#cd zEo=XN#Po--Hd~b|_#%2q>X&7Z226SQVHQl7uEvV0yyDCZea_fYqXWm`O6ksce4zbmo%qhjH=s9KQF}*~h zG)m`ZZ?T1{)qa8d5uZs^!VoU3N?%Jt&Bw?GkFb7m{Nidav5-sVp(%_gC_{AEBl;~8 zZLyc5cGkU<;Lj&hVV_vm4u@lsu|+3Wut@wF-}XVjRNw!Vk_dQ4qQD<|#(@v{PZq?} zhw8oWJujN~P=j3m$MdQ}v4zCWqg#E5jk?ZS&{jwAk}k$fO_~3--j4uT$(0gxAdQTaOeii$EB? zS3sk@6=%a}DTtq67XvponT3US1`Q{!@J2JI)i1&vp5NviWw}J>^|L;N$Te4KLN}Sf z0b;>Bmhl!AJ@aR?#uhsJ;er;Z2zV|wj2Cy9cIi3pwz^Lq{z8wRsy~@rYWIY5l*p9g zw~@6>_H42*JVbO$YsgG~TFxyJ{UzRRPxuHR*9CNnWz`Q?UOkZ0vCoTwk)HCACYX?i zh=N+8S_3UF6QzYx=^=s}1d&;Vl5rcAS2(bvf$qzS>!llv2NKq^j9bwu!S%4}F!O@F z_1Tx%RUg&e+auJTHB`g2EH*N^JwQe5fG`UpSq5Su?(r*BCtkwp9?EFJPK}jGzchTm zgiFlnXjhuaJXY`r#xHrQ6DFesTt<=0itwyp)ZOb$W&>6}LqmGDZY(2H)N)aTV`yQ% zM3L5_T1nguqHri0Ht49_3k_P_-I(kw@S=8F|Z`~uJ_hz^{CkE z>+>^rJjShGxW$2)c!R}P?KBZ8A$EZLdL)SB-H?I_$y~+JpndKZc3hF=?R(n91U&*X z%}Mt_p@fo1x#)cDAcYs3Z7QBc9A*6TCL_A{ho*WGFBnbsH#G;4>Q5wWMdeNkOzXF* zHaU-0Puj|~@Yg@S zRLu{zehb=nxzFQ!%yEoPR>c=Z-J-$RXPo=YBS(2!)A!i8Oj83ixjhV1dsZi3rOSZb}hTBKY=L(U!&Byhm1!yz%Rz zOJjO?@J;FLiLB|;sf-5?%^(|I`p{}GNMTV43+zgC#&XG%9mFMZO{>^*--SBWq@?S}9DZc8ZwPxBbx> z%+@{g5i)Qbo=2=BM(jc4s!#IJ?-RM|7BgC&86_GlOfDcykxDNllM$!;42V}Ncn3nl z=}6ZA0Q!wlrMz0#AeNjoQ$Nn2%9jiB-AG*8m%LfTwJE<3pHHi^aHg@02x^AtH+t|{ z-u~D!MnNF%6{M027OgyAnI5Ce8CD!>n0U8lnfh6xg@xlwpmoxXZZG4 zLd#|fFQN%A|IIL{sz8GXot~uxO8;5c8uNs}S~?hgMyat(a^waU1JgfFI-E#|UM=Gj zXwS$;bF2k~?GZF&tdEQ2@Op(8^ax5M2&)-2h|$VHygFyd*jX8fGDH>H0nT5ty3+Yw zcYI84CA9u@-rhL;(^}>HrS?FK5q2>A2swG!L*0=1(mBdND$kew&^owGk^?3UlwLZ4 zmMG7*=UhtpEAsW)4pE~3_5v|(0e zFd6gdd#l&rtPluT{dxlJXtnP5SiFPwVjvM`=NSp&j1BSA)NWW2`mn=Gxw8j_sUFYd zoDdJ(I@zbyBq6^JLLIo6$=jDnl@SJT$U4=6Je?1c4`{F_w zu?Mr&4IBXD+H=^ITK6$KJycvCxXS^uIZCL0)h?j% zt2c427W`&}oH*jRpahB&VocZb;Gs+JMmoRs1BV+UcWY*X-WUH^wvuGsV1}33H99LQ z^j(r))KkVkPpyx9@?YM@6XBvO-p6UXlix6ZppCi+)CBtZWr~@7@?@8e*?Bl8`0bv?PI$Tc>2@2THL` zTOJYY=R||r5!+XYnHpYkFc8!Ky77VTNz-#WQ{`(IT@)wgrK7Kw_cR=%X7&9toXcBX zf~~DK{kp>K;)+9~md*Fsh}z6`XXJe}$zKHBl&NI-#gLbmPrDYPNmo0XoEz)0I3G4Z znvt5b4*;5gW(seXen5P>{91xrH#3y2qgM!Z2J<>iJHXE%jghHvPU<^K-pDS`pNU*> zxwYbscr*H1pF|Syq8JS6V=Sj1oUYvo-nW-(9wb3e!}Ni{Z)2Y&sI}cJ^liL$WuW4+XJxFgza}%qBL4f3BDe&a zysX##hZGb@fv@Wd6iy!yHfNegnD#GOlzHpvm#6)CU#Vx_>llnrlH{*O$-Mwy`RK8{ z5f+RNZoovvK+4QD#c`s}gNlfUeV71t*uPG}Y~kIwch9#;Xv*kRkLuAJzD%geK=tp+ z9mk24ZAf4Cojhw3!K3_fapi&KvuMn#B`D!AA{gM#O0x^t=m8wRwgpHNjA6>l?sW1GxYL$stix; z_b5;mjp44pUB9t>4e9yt;kteWyM_ROXwu7)%7g{S@uRGJyyh2dVT{V9(} zv45mFcR~D^HHu@I;|IjJrG+zVm?na_LBc#=@WUC+ZP=EnByk)F<@a|pCh5X)!Pv%t ze&_7@gPFk%7mBJ=&SDkp`(&R1PC=)2*Vp#HC0QgR{XkQx`f5*N@Ks-dOZrJVd_PM) zeSy|jB;rJ2NZzwNN1+1?)67lOr-q`8nBm|WH#Zgdm%7`{MAbv9f8&18;P=xs;-`ah z(~E~GFT(olV<4VZgRp|+fraB=r?ezR?i&DC@JIjb&`2#X-z|A`{PTfZ-`ll%ObEZ& z$C>4@yKXQB-V{7a%4cU5NMx#NRAwKf^J%XpFB?9d9r49#7zWWsQ4h$6DrY46uyY_8 zp9*r&HD%d@Um@uC^$HG|4ZZJA4W9iKjQ{S5To2xHpFk+PnyvI#{P1|8c%aiJ#A5gQ z`l_tz^Ee=`e7p-qC_KBW7hFiW50!sUuj+QO`UdA~D;?)x*53|MXKIcD8lrN;rFIY> z=RRkT5Ynu?G#V`jq%H9&X|CFd7Rnj# zQM(_T+fy?Ur+_C-h)=7n%Z#qdBDk(0qk2B*6nfJ!JDq9{L&Jg0we4G zp7R|k^W1TmJ+)=uY6fEs0`Sp?XbfH3_)cqc5>tgPNl(MDY{I=3Pabc%e?n04#+!{B z?sG=S!*_z@kiYiQ2QNudTb!z!ovFWh=hGC7kc3oCRK9_15>|wdv+H*Ug8<}Uca_ye z+rP*jeJMIi3~YzGXa?`?%Q!(4US4D@JshMZ3qZ%}GIn2{Y_@CKyvxYNACmZj?QJL> zF4{Bki1FdWfjkp*_TwQxIK>e}U#WnWYA=Zuz0Cm}rWyJBO`roxQTZEaaL&7$MzDl2 z$pM${lZ~C%hLWZFUJy2AC-fcgeh$DX+(4FuZjbD@_LbtDWw3Z+%x5`e&^(5JeR)-s zEC%-UoJSP+s>VJzV_?xykL$xS+q$dmWMM0*L@R0DwhR#FIl0A2kefA6=F}oq>WZ5i!(@n((T zxmcmt3-FZT+3m|qQawC#qL*ni$khYS@wsa!b{?Hr6iJFQ3^`-VkEEYvC&)EkXWBFY zayp0@B#TBbK>$e4!swz%^)^lwVSnR?4NQG$v1Y(#x1J+%C?GQem z&abf?Ksp$xNxh)Inqui1l`IkEH6=^h(w?Nk%n}JuzJRm11}HzxP$~{tT8>-5vTEU$ zWXpO*7SVCp6;(Ha2>$?27J;-voa2nHTDG1t4p>nf#QSHV39*8a4 z-OvoRnAESj4y#wKp^i`p&^(wLRZo|n*&hO^vZUkDQZf?(cLa%RQ~qMCB>p4J)X!05F-`yMT|nH{@@;Rs0YS zf&gw(RyiN-Mi1-R}iy~(_5aIP>rH*~s?9?v2wF8jCf z9-OM$9X-$uN0Z%Q7(XBfS2bj|R(VdhKOP;1gB^_5*L&q8LD^Gj^>v2Z#qD2jV%w7WllpMSlAh_n=?+0o3UOJc60jsE|WrX|$B5-3vobDlP6J+m>2I~2e;SP2K zHA9P^QMWO?8G`$n_v*0FfU}YmDXU7}A{f_PRLqkxC=;ohpp8?%juL!6j`oOSQ*yxo z4BzGQzR#ZjjJ|(m`%Y67q(R_o55I~nq0cTvrf?+3go}8&d{p|D%jItECQ(C__AQ?| z_91-X(a$N3lo~vq>}vb!KrdofOdlbrces0%u&->>7jC;kZQ(*#R?3UN)2z$5axDxS zz!_G>!6(A-d+nRf+gG#3qROTkNN*uu)9#ujs846NIUAfkBc2Qe;d<0RRAO1@ao03W z!-y*DxNE_$S)Q3Qtk#Nd&6XDMbXeB@ur9W3-cgpMt#J?oy*d_TPG>L%ex5}r?M2x| z5-w1vc(4m$KmOO7ATk!)s4ppNbDs zXh|MD$WQE(WwB|VyEI(}VusW_?k?p4;ejuu?M0BcKU__%Q`s;-#6`VrqCDQ5w7u&z z<=Lq|)V52}B-9k4P*|!BBsO`r=HF&v+YfD{=g&Fwks1tietQGuFdFNBBXBLOCI;~$ zlao4E%ahg_G*8Yn>DT1>u&ok%wo>4OSY328ouOIoUej536Jd7y?EU#%$Z0Ye!>lei z;)Q25jrj!Q7_1Hn7P+!XujcWdy!nSg;8ibl4o0a3(Qiujlc6?KhpkS+VxeaC2AS}& zPb7q}0PO;^@h~o?8sJv?>}kKn+~yI%1mi?ldi&j=U1mIgI8_hc2d@_+P%NXsq+-zd2l_q z4_?)3&rfuI7BI)hLbt?cEj*1spT1q67=CyNsK0lw85SNpM4m7)v-v5++DFUQP0a`M z#m_|uQ)K$ge-r=Xowl`l5Xqqzm#1{Mi9sIP;eJ-R9*NiOZOPcVA0iS#@7%{F$}pet z^CBXA-3V0Y|GajU=<{+iMnmb}j@4W`oXEOS*Y~Qr%RdgLCFtsjM_0l-ETc%-J1TLQ zjM{*lks|WCpVVhZf+UOtf1COXy`EeN4OgZP+M3Z8&=9Tsd>0P5PwFGOFNYWr)`jOI zDPAiO30$Y4t!N;7g+QU?zIt!04W*ko9D}HsC>G=r8iJCF&Dp}e*)gN|aRQC+VZMs} z1-LFsw0CGu!zZYc)g718*zxf8gdEuy!K!%QXi2Jk(co{tm$ii9giQJU@0@@uc|ewr z>6_y8~}Ay+~?t15{uTMt1`ib`7APqiuyHz9-{0j9V~Zm zAP^^lDx4r8h&Kv3{1f#{bX9L3wW?9-?FZD;%v+4EU{5M(mgABu$)!p)+{+SZ?l|`R zRhGD9g|Y2i$^ieksfTrVcpf-Ivg)>`Y1}?utD1669&N=J`tyn)`jc@xnj*Ufq7!;t z*=MEv_POhdg$btDcb}G%csJ15xdlk=iUPaQ%O4y1>tnz70;yK60XWc`8o=CmLYk-0 z-$hw59E=gldLtj_7#1|l6X){5?A_h}>)m?yFAP3$C0*kzJTmuTj}lBsu~e>t0R-to z`N?DlEq^Te{l%Yh6mCsc=5{8q5jF88cYE=u(cz&j}fyD*#LRZJ7%5u{Vb~|tFQ`1F2m7zd2M*h zN3)5`F9I3p=;`qjx@ixb`?@H~EZiDoD{2$lUzQ3l{54Kf-F;HHdFA73MU0)`!V@iw zCQSw%O=VUL9Wa-k+~>64U8{(gvOuNQq790>-ZWKDF<+Gf4Gh+jqJ*T>s*;*t_CjBT zrp(f;n(5rmUaDWzUaxO+nmTs)y0_QqHcxX)6(Cn6ikAeV!-e^y*ar3+dP;*$)krb9 z=xV%Z>RNvhp}#xRu7~l=Lgm{Dw0Hb!g4pJbS|8@OwyZqhf2N0gn^PUeK3%Wn7<-}D zRjz&fJno{9eoLIz;pBeC=?xPH4AQ@4ErIKTRQ?Sgu;Ob16DXuM{eHtgHeglb_A$-Ez7>KU-|VP836uhR#<2h&P1Q2q5hK2etSs0`-c)RP+`48;$2vcNp5@g`274g z_6nibC|I^!er#smrTQS&a0X~}cCYkWJVXsprbNV`_XG$l^e2#UG|e7TX2C&*&e7^6 zQ7L!B+;-P@-OK%uKJZY|5{*2y#)3auwEp$o24?RKI8}Zgq*Y75!#EY}3ESj_ZdKrDZ zIH``xqn&;M@&b8ztA+XCI4kg5`zbPj$Q3c1U+>)LKlIxNvyt);w3wAAF{2Vq=bAuE z@uY#2VLQ{~?;QHjxy{~F%qprmNPyV<#HSY8vfx_NM8CP6H!G}ZiMd)6|9OmesONvC zUg;0Hxcit`n;$uux%q}u%8x(zzj~TUkZDK=@yxF|&3|V;4Ylo%g;_&3fMyP;_D+gF zHUgv|6CO2|Nl5kNp|DXAr#BuLwl8cI(E4821ePbbW2kwJlmpt4uHV)089Qk3CUD}= zKx9Z~OBRR^Lq)G;VvL(v9d{;8hhJWv(edghJhbluO**u$me83HEhCO37LfyCNwznK zUxpLGF3eqH&q`4h46j3Ii*W2_f@y>NqtvIb2&_1AkwQi8VlfEq*B5hi)A82*h?t@P z55AI-**v_X&h2@u32@6-k$WsewPTT2*I7%YJJ-7|{C}9bjwMH#ApJlb@a`xOMtBhZ zh85m}fT!P5&mX(nF%dH{-7TT2%Fk3~Wp>FsO)cxmHmmFVdE1#f^jG`%W!|E2dbGoL zQ~-2iU%Gx`XQ6^l?Ht#KeL(X)gFhT%DV*e--u$RbEY0&5)Gyd|r1h0&Fk{3^oXIft znJU>2|E|uRV5$)ky2w#qhGl4)W^XwzJ)k2O7i%Q{U!9fZJ9H-|^edd~yKuwF_)WlU@Frmz8hrND;Vmm;CdRF84%@Af_R|MqXy zDiz+Spk`!?CDuduEJiPKw#WO&Fnxag{6fviT#;h7!H&8J8@mtj(W*z|aW4=*AgcLS zcK&p61*4~SKos>ZcBLU-$#dQux{Syn%D`vu;$LiTEcwEHCliYPvaf%2mjPG(?kuyuXiaZ*)+XH1ln2HEdQM-QZQ%H50vP!*{0_HRFAX=yg0vyA>-IyOj|=vG@69^)O|{Zvk{g5BpN7&+#h z_UOW9YyfM$Lm0L-ciWY4T>Va4i*m<^{g96rSnctwu*n+MWU@i+7yzhIli295)U=Lm zWy)a~(A?($Uq0jKhd6_fvTdTM0hpYPrD#5mQ4x+n&|ZoPLi$4CI+}2kFAjLoe(r{P znm@nVr<&c2-{A(j+2ca<**2c63#e&ifVC$ES90~D!mwU_nRiiAvZz`#P3o;H)ZpWX zn4VYaNLye61FOYMlhkgM7v2uz@Npa+4kPLvJBvHmzpGfIZoej_H!f(DD5~n6fP9*D zG84`xh(_t}9$j{&hVT&TrTmuzN#&gc%TK=-l9o+Ek)jqPXbY3#bMOpoZnVB$%puS` zD&-0edN#~ZGZqfVIgzt%HMV&A95j!uGZ>2c$Qn>;q#fM{X8xgRn&Fub;PH)7fP5YJ zK_9#q-T(U9fVdfq-THj&BnlBUlv3?9+@Z@F8`?WTFqomm~~nhvEO~8wVfl;bz~dzJ|y#A!Bp8r zCp5np@`Ntq#6*l^L92$_PmLKaH$_J3Yc=wvY)rKJP|&y)%LtRA^J*nfA(hNU5h@eqH#G_(K^ zYk-?xmOvtb{OV!ULI&I)z?@Y3_`?Ti^PK%~S2+nnvXj_6IMi6EZuJ68BR8}(WCyc^bn75TI|;zR%Hs=%X& z_@>xk2PvZf7EhIy&kf7n5ORg%yyAye)sW^e3YM7xMIEqeO=&4fPyM4xxN-K*p_BoEPwGV=zT|iiO`S<)Ot4l%1viX{@3RvDDRm0 z)xi(pb7P%f8PR20b=_>)`nR@g!Kh~qyI5Fm$ob8Ba7yfuXW@Rira3web+I@w+%t(` zOdqrH(=T(TzK`o6oS(DuYo++!EE0>nVnWIVeYm)oBt@?La{JM&np+2HV4UkQtyNOS z?!j{J{rZkajDU(HJqy#aSaNi@uw`2oh9=8}FNZO+KK5U`I!y#I7kx20fdS-t z4Mr0E5v`K^wqCXHSEG#n^>kD;)v`6V6D3&^=>KYzjS}pX`+&J6`Fz>i7F>`pmn4lx z&JNjR8KN%9(6eC@au&eE19m&qh3>RXN-)F)CzY6~4Zj7#6Q)c&7M~(lQe)46;^O>zJ@fsUFNL3Z%@Me3bYuL3!6Pv@Cik;H8p;&4@ohXPUoIg6iIKgq*xDv ze;8qYbTEWxn?+&&M8#`J8mgA^v)C=5QxFqUVoS3HymUV{`5Vns+-l zF`7v7ba}l#4qEUo31`n}{Rgu@9O zpTxrK1sdojL^eUl5^tdC6S;eAUrDY-KhID8xnD#me#u>NXDmT=bX=wwX0o;}afqdv z!v;@W6j`2S0+_qc^Vw7ng4ex+KynlPorM&+U0_QAmwVPbIJ3KoU!oy;jNNN2tmRi< z8JWDW?3zP%oRUuor|&AiS);YZn{?U(tbnU7CPaK;c^RA6dm<(6yxD1}?}7+fPcNSW07 zP(Ll{faj(0IItQRI)_=B^eV%^deeftm{aGFrsLcoHJJceP@ai44i`{!&RIs`Y2#w< zc#_m7Maeb4nGm^*n3ihaFQ)pvNsSgjProgB*bsP zc;D~&*)F-tMRfDh<~*heM;&uVO%0K)KIn)-wSNXyY3`t{(3UohGT z5Hm5z@^4>MaTkh$RI`J;0bfdIKRgdfmDDGPv76J7vxfI(fN0U4TrT;C;Dhx)%|eGJ zKMFiy@hYpemzM^zcrZ3FeL7@)Fmir6eVA^X<(E<;q#f`!om^T4jg!8Xi&O+fB$1f} zmF1Y@F+Q!{Q6R$9KCx8?)M(9$z@yY2u>Dn%n?&!=Z1&0`7;?(930TG^sL?u(b%kD5 z3D?imR5A;uQ5#3<4P$ql0(kLPH^Lc7!d3)oDXzDCe#qaQ8{>?Ew?Dz8>yf0y$N$w< zk;cSx5cue&K~G`w6#{N`J2EztEGu1j<>bsz;_ zE7P>u$&q9*4-sY2&cx7q(?hSR{ep_rO@cVUL+$j!RAXPPiQy~OI6wyy(dJ(-f5ycN zO|~APPIEq9roK!YBBsnFc1J3SY2j|WlShfw0}RL^Bg4G?_1G-7j~%AHZl>ltiXn&t~n z(6h}&6QIJXKoRB(*HviYJa7`di58N(OEp-@PS$)MjbIb}0PjZCCt0V=6FF9glTW~a zWuMJ2iN@zB5jnF#V^S0)xJP2$KyMXt?ILnMgGr4h$rpWc{tz2U%&-f>HeFvA;j&-) zYl*Q_lo~x@7(fxG(6W-~t*}Sd(hlG6Vcy0f99S)ISjfUes$|coD!aK*-=cCEzfFEG z|3>7v_xIPQV`7jItDA$5yrv6{bY;Q$=8C=Qx<#?n;zG5NuQ#doZ&=lHn2$-h^JPB` zEIC=CTzyg;-1fAvNZd5pu%xyMj_Q4z0!_pM%2VZ%<%ueDJnVRJ4sl|h*uRlpXUSim zf^J5ePCt%c^53K$-r#$`TB0qE15Xj-#n?Srl$gi`!rwi$e>?TDRTwTb2$o#E30oud zYx1ZN?cw;xi67TQj864BCzKBt=y&UOYnt=q%r39%U4h|qSsMEG@b;{~s}MbbrJY*} z*_M4=fA_!M!VT|j-ISH;$sYH#fzw6bxWpe|p_ZsGqj3n#miSbhRNS8()-hybEOGqjkSPPpMMMNIpQ)&g2)YYuR#{hred$cJ%b z>eunzrFp~Lw)48Kg;WFHDy#Q7or+(7U?yIEscd%c7p}#0_8E8+v`zI$j``;6>*~@z z7b@3da`5iAFRy)IiRs?uWiz3`3H5o$q&oP*Q@7cQSB}jo;OCP7qqCwk)*he$%?jq-UCE?uAbAcAn z5Cla_%}w&VU+LWQJat0xJBuuZ))G|RSwrsc-s_=Kjnu1}(~uvX*x;|a$<0YictKLT zUCE)B|77RCdeVSTfk5KrE6M!>#d+&jh81$ozJ#HbwfPr5t!aXiS)A5e%yMH39cdJt zz+Ab~O`g7X5*KTV9m^cTr!bZ$;JD!el5uuEu`z$dm`8F=VqM4a+4L`G>Wdi|pLQ*4 z2ho3h)O8F09SwGG=%%->G5QgjEpiE*X@)R1z`c!B;5tBaaxJ$Fc8}-rCR)w1;kx{t zy^mXA|M~?=G9^xL!u8&`PL*(pFYP1nr5u7Omv9CB^<{j)aTrCAoZs$*EGfds6rPvM zqgV>IFd~7M{(6dwel)zSUy_fX%ffkj-*7yov97Z~2_7e^DgqxiJn?;dne;k1M(zsX z)agqKUPC80eP@Cd{@SrVGsijS>1=g#9XtLR=3Rw`GeF8AV>upCClYuDms z6!}0+O5Bmi#bB^UC#Ny_ZJY^zdb!KAb|O$PcL1TB6a(t_!v^~?=1p(th0@s4F|}j4 zn-&6-vxwn1$VN?apvLN=EQ! zXoaPJ+@kwy|0;_{8SG zS`?bY5HGRCPB7-x%dAh>eB9A~@U6O6%~MU3;&|f-ylW_5CF`+Lz$Q*rXdn z&J(F$jSk!B<9hCL>1P;fIqrc-3_Z|?2A1McL}>aXboZ-}%E|$Z>Gz>bE-$Hn=P`cV zImfO_P5moy;EtX($QDdGz8qVS&bp7s(ZRrAWIur3F`aG4DVC@an<>(Il3cL=oo3YwzU;Ul7;Hw(|GgBLp-zfWmVK$_%y-Cj^F7-dewMQyt(#u~(z7 z1GV`zQQ{*0H4yswt%awNBJw2|X@h3_`XSPMh^sr<3i>-*4+>9>da@qyashVXZ6kL* zYvoTi4gAfMT+1tC19F^`nraXLQ2DAM+Dmo0q>Ay5jsc}#Hi`Ai!OoAww-}tpT2t0~ zD}z7m6Awu#&33EMo09fQU!`gHrn(Z4%(EhM78{xQ?BGoU@-0P{G8WlnfT`1cz%Y@$ zoO!!Z8DU2+X(WQoWBPe+)28m$&W+Z)myxqV^v9FqVjfDLg&PSnbI8MOP8Z%Zj{ZDY zV1C_(gdgRD3KeU|!21nkv6vx(2AYl|S)x8~w3I(OM|KrrF+NI?Q}Wrc94sB7TaCe_ zCt^;^^QIZ=LI3t}#CZKcxXtc4Di`j6g0cYrGkAxS@3az(&*Wosg*oKuV`d@(Si9qPbm4&YI3IEG5b)EtvUJ= zXq9^i_lhnv0#0T#SS>22=lO~K`nBv@#Gn*@=Vfi#xAF5dRpPMgLP1A&{nf1+!Mwnb zprhq7xGXiohxV@1`=A5=Nh0)RZK8w&e{?#*6Xwkk-T^c|)4jnSB}CIOT#m(w-7aKq zs2ioX)8PlhEQYmHC2!jho$vS@X|Vf@1NUHBD4kQ))ivQ7D}XZzMDrTp5GIG0Ee+61 zMopSXXCX0ggfXm#|IiFFY;>sHyXS1axLL}0*tvoPE4y$YO~bW>9K>N>WB_&rSl%=f z3%xD3oGH1gXRBN2W)cHg@^H%LY%-_*o)G?oX-1F>b%sXB#pG#EzK2gfyISYID2689 ziA3i8K!)5>_VW?!A$dm<$}x`u)tHmWcOd9OJk~9VD;?erYnFO*>~=lhO`}E1SWG5R z4G=44!9+b&6@U%PiUE?6$W9z-x&Qk9((sDvHxZh(_rR@%w@+qCV*aD!)*uXI@+m&| zD7`P16TVJno!oj6>cv5x+wpOqOm}^Xo#eq&aeT{8z#O_g3U}#DPm4&_ID(c=OGUYA z9mi1@f?diA*u~e4?EG8oa zL3B=OevbE39go2g4w`!DGR-2vs_Ss@9OZMxc+Wql*yydR-X`-K(WPN>x_9m@)LT-R zpzd>1p6@F0ZOy@H?>P)^QcG&{eYVKM7qU9_1SCCtT9c-?g%aPzk&ZioS?gJyW0&K{ zZEx!5$d#8U@S-{ciT>Smd+|}@(f2nmCd-TIol%*D;De3M?7zrlH)nTK?j?Xa=fzL{ z`PrTy;e|d`{Qp185|;2yGHl4ifAy;-T)Gn6lJ>E(P6$kSwz~XNAy{*y6P5;GOF| zmzM8e{`sVHAcEdWV#y_V0vv+g)oXZfTJ@{o=bi7!ZoJ}P&N`u_Sts79+J6y5Dbw60 zp&X)S(!%f>n$^;na6DC5#mJyd6!}I+9&Lsk~BldK2xQ73DbygOhaxf^|UwoGG)8k;sFc_vy zII+SEG}msP)dz9Co2<;tHfT7yrKVs_E8)aD1@;Gz`*m&@T&BG+ke~F+`hH4scgv(N zzF#<|1{a1Cl41;pJ*Z|gLcrWU_~8eYuqd5@HN0?D1D{wDAKEKN2ctrfUAD=hb-DKY z5N4KoiNU)JaR$dr^SUV*jIyHX(={z#YP=XKN*lAMhL_kR2sJ1y`|Enu-Px->u^xwq z$--vsa-*UOX6)dtp=*k&9vN*7i48YqDLWjR%T2$Re11Np zINs@-ZsdFWszp(D35q?#U!S);Twt0FyEJWkNvA`9mRkajAjy4OI>sT$w$BV)+&2X= zy@_0Kg?CtG1~srSeP`3~$SO;X6qPixWxNQJuVdr)`^7|cf#hd?09QlG4X4^vZih}Q z29$EXwCAOD@0@E*#Gb!}pF>loz89-PGzuM4W2dK=xKs-o(n%R6A2ST`tMf3Xb7zTE zHiIDr%CMUV3P5~8U*vpWiecXwa9r(+kG&B<@!9n8oCS-Fru@Nd1wd9bVlQg6B%Qw= z@~FR6gi>p|(iJ`PB5d$ps0w@wnDdFLroOdRDdB^-w?FF1`}}ye>DWgcUE7>M(M-ZJ zy8vSZ?JVxzy!yKPhHIFjLcU%!0_TrU=%|GEdp^Q$v-SLm+(mjB59?soWbalP8j$CO zdP7jKzs!0Zrc4VDkWxg=83*CUJdYzc)d@hGCY!GsicWuo-zf(H2bZ2tDf1 zSE}sW`1SdyDWJcm^{Tw+tLbVp!f=p_{n6X@^UK4QT{bBEUQ3Tc>&-L~s=#j4%U+)c zN(;T4whRK915nGO0kDagKLZ;k(ni~9y3`fP2N)Z9;WTq+`Lx~*Hb%&vdz3xAAYjM;>DqI~@+O4Mmj(5A<07_g9QoPy=^f?Dz-IoH z$<{DWm%fTH;J)PatCFKrtJ!&2ZFral$^vsK&ql)>^4@)`qOLT~DWdwoi@@aG(``t4 z*u8qVZ#h=%(}fjYBQ^r1DutQV=01=l%BnNab6C<2o2&r z`_rKn{m*7qBW5@8z&LoGgU}%^JM5a5drGVR@$Ru(@ z!UGHS+f1l7jP~3ZLh1xxh{)TdL1f7)pV9e1Q*qmvRs~?VFoI?>$74utNgR6Kp2U65Ecju0e8(fKFTBN5m7fv)J~Wh>uBdGVULA*d{)e|6n^l?}h2ww!d<#E(gKD!q7mV zC-y~N&BQz*l5GPSr~qTGG|ETt*GG>1VQpen=^;ec8XF@$rH)Jz=j z1OTs$IcTnWy~-nbUlGjgH%uB8%)~s|zc61KozFYWQ=3_Q3H`-#)d?9+oE0p?31oe5 z+PQu9Xvnm2W1{d}eWXgn<<>MaB_gQZX&tOtKr;zqbhAQJ1Fc61y8|8W6%g~gVRVi* zkRIYwY^6Ac71pBduAD=$@uohIHzdkinqcT=iCxkk+|fB_sy_C{fL@C?ghfRXoo9b8 zcduZnh!nHDN@#TbE7kpAh~Q(bO+yxe+F-Zs$gi4GeHMb2Vj^-?9{tBwI%w3rxdVhM z#6%G6K$&mM_ z|Anvt%@b?xXqn$6RRWgCKOx^rj@XMzd6$*y3i)X>-j=;DtJC>M~Y$t9R zzj@c}3aTF(VimT_HuV#7n{&Y*c-F!liJA`Z!zo2{{Ez4H6vNu_J#Qb3-?GD*s47OM zseoN{2m&TXJY%;Kzi=|K*IrG4-sSG+gfMnrZ?4|6mrur?C6&ixZolrX$0Kgce|tWD z3%2r&Px9sUO4~a#H+DyOG|XPO=cumKP37vXad;NODk{#bQ6`uwMxM%XKjQw_ef+5& z7Bx9n8psm}XHz5t%^Sv0`qVel5moE&y z5T+k2Zhclu)FswgURrh1OL zebn;=lp6h|li8809ni>>LIYPw%!rHmx6^dum8g;G)9Nx0^>*x6-9kXa$0(No4ul`U zV)RWZ3$Gy8`OZ`Q+*IC!OQd%eol|>G-2>He@Cu`wKsk%&IJc4DJAl=zgAJvSIyP6$ zC#Uj`$ps4Q8hejsJxiG%h=eMJNJ>ZW|hF{5lM`8L1~s6*S7tXA7Z#YGTtCp{AxbM?CPg* zEy0mTn)$FAz9KA)9%(&8=y#&v8x6(pf$X6+PL*)n+2f_H%r%zC#>?{SZP26-%hthf zHFF#*O|9v0#56Nm{kbO6XY5q~j>g3G2X1Wxospr50Y}y`*a4S&+YEh_`7)hCALzPq zyQB)^VsAD-d-+)n!3ch~@ksYv^Gy+%v4U}`p;d%E?knkPp+?rxjhc}y4ccs zP6za%<*Fy3r95a@A8zo!sX;n#<1sq35kSd_n2iG&@+Wg~7`CkU9$JIl^h`Vs2voja zr?oA>m+R#txnLk9U-|%zkw3qYRfCfB;Unf&9TdI5k&T5i!%`tOJ$65m18Q4T^uXY- zELFenkt-kgtudFh%L4- zpE5zYlMuvDBx43F9Dkp%%XIJQs`jy5}D)qVSQr{#c zpW!GXgm2S4*ma+Cbvmd;b36cj>XZTM$&f%S-aFPOttPei$>`;WHobR^fb|~#(wbz? z`{kOrcb+d@`kC0$3Cd{XOrH`;KW+Hd)4-rf(HPv&3-fFMSX~pIkxaU*jf#nUjUYZ)wi6+B(>3&MJM-IYfFArJt`v2xEW7luFlKg{2(*B(f<1~NI~X?J&bD$3J%dHgq3f#5-PW`dl~_O z5*?BAMr2U?UK9o7>N*2?6NvD4MN$+m`2=KI)+u(NdVjX!@XuPdJE^`&HCiqEppcQv?M57;StiA4O6BEi;!QG^p45zx!lu zz8%Mnd}W|JY{Ay;hf2N#zUs$qj(D&%SAI9_we(2ULusS1jYNpROz1H#-9{0OTd^PjJ=TP0bq+ zIu870VZ&SDy_JHFh0J*F!D>JqeKfJY?0+>ne&@OG1bp!8BEuUV6RBF%)Qhm&W^F+w zzK1nrNn|Ift{u=3rsxY};O=I6lUc;(T9R!mNEQ{6B0qEZO5qNoF3gPG1;qH@{+oxqe&n#Ab?-`PH^3!f>vQ>c{&VyscUQSS) zo!IqiJ}oJ--@%>jONcUpL^GlHw};8dq+@tbWGFY>x8is^2ECO@wl*Zc)YlCRxhf4g zA$?+^H1;lnbKLgkB|^iFuI82+ql%{pf}l|$>O+~LX$5Sy)Zf01fBU>& zuIKqKX*Aiw3)_$59Y+d14L5-P8-gMu#jmSAx|IO{jBPIqZa$wcg)mu+lO&4I-QfS~ z>ADPtK^;zIF!XrX27f2+Y*_l}n>ub$@#8T8=;UT0*+XBdQMD(4wa8IZ>Wt|6E}>6( zO!4A)qd*h)N&)Uj^REZNk&m_!{_3L*Rpk`kTlafqGVC*7yps%i|rvQ6FQ0K2r?2?(L5<2_QDyc>N+5i$8Yi$&tX5$9PmcuJ2avf#6~* ztoiTR7~fie5x?h1*ORlWpoGTElCNJQ?R^g#?UNUS1-;flE46%g;9yVLFa+LxDv&LPULz9(sw6ynW$ z=sfDqd&V%7dhgM7K3lB;cqo?m)x;~t^Ee(R>94l;wUnZ#)`3Ihwd(Z`AV0Rxuf84g zxL+-EAhPeH$T#Fkl`J(~4ONJ<4|G zC}a;VAj)~#G8X4baUZ4N*U1u3K`ie~_p8}3ipY{IjX_d#Qck*5I3iPI`L0BxOADe| zae7kwj)p%>C2MD(eq>7S7!ihxt#nE}?<`7r;EQ%sQMIg0gt@G0F6KIt2&5w{zbNhfL3nIY`jfWl_S| z$cLum@y%gto8&&)myh`Cqe=#$m5M@;Eb$1StK%tH|N4qZMCfqo@(#5SAOW={|LDjU@v!f}G)%6YV?suofuWY}kkjYnd zJB_yI7y#=2Fc742bH?$!!JOqaSLZ_;{Zmv7Z6-E`gpvd>)N;+=zs*#K5*hlwmf`w|7?wLE?9Um?TD#G+}k0*OqVw z-b-cN8Pg?{Iv`>doDXVVTN}w>kft_&1uqdO2srsQWJ}|uO3-OerE6qoT%CDO4~CLD z(Qg#QesWJJmz&VQmDKCa;7IZ)jSsZ$V;TIk!C0~lj9CN%UBty^0VWs;RTdA*g)4+r ze)*1VR^l%Z!n0J)*9F0$$|C%PKykmEx1NY2+mMtC6^~i54ZHDG zK6lS#RbjTGbkc(Y^GT8@q2>=*wp_V-1;M>#5rgwMk3t3t519%_!s7F_s>TmVx z7O>_Q1(5XzeTc{ah|bHUU!jd(|Dz7c3c$d?j}fF|lV?(Sfm>L|Z!hRNa2!7&maET( z*-Th@f20>7yak9K;8O`7nt4b7Q@%bh9pG8^LQt?WRN*hWfp~H&FAmxd@R1G4=jHSB zb(QX_-sty4<>ii0BN;3jSgaf^;j>%cocYGme0wkVmmR64ye1xJShMcXwZMBAW`z+t z11iOy^gD6bWPZ*pPd|S{MarkPmdt=22;mN$3;DRrcP{ltqx3Ln`g|6?|Cp)0oj>Iw zi`#C#zP}tIxSL9#CgkN<$`D-ty&jck4YsF=Yl8qk zto=mNm9=s&h@+py>!Urp^40^$~qUjqJLgkD`hS#(cqw%XJ zCj94OvuplU2Ff=G<%eWDNIlKgk_y4QpEvCZ@GuP9{!OrV2p?}Y`eh?5{AxuN5I|)z zN)rJc$eMxG1JQXEju-aZeBUNTE$3myDYT%Lzdk~18Tsc;e9_YLP>ueg_83;L_XLmD zia^vf48R}0HQFsY8jEu5lhT_eOhY@>jwhWsl#CPO54vu!Z>sZ3OK0V1vb2#m zDy*FZVa9iDnx?L~)_5I|^tR+OO=JguBec0>Qs_fQoSA8d7Kv=SWjq1wAHr1JCAl!Y30xqeJd)J87g}YGRpu2H1Y+>xbRPgST^g z^K*Crf1NCTp&lEeF0_r%uKU;4AX~0{W#w`l3!=D_ilI&3=L&f9aga4-p+kt%WNXgt zXSTp6XIL5sWWcTtIPe%p@u|uJE3Wl00Mr%M)Np-Dg=JDL8~QN*@&FmZp}ig7>1KP= z`R_&vs--OUf|4v2c$$gk<3v-O>|z{n;A+!jG+(^Vru9U?S)9LK8N4IqC6)4^=F9@U z2!A@SI906uPLZHB_Jn4qQh&8RgPWSG-6C6R^Sx0X)kvN|01S~ovD2b1ws}Kj8tC^C zSJCDYufUM8ByW9l?2nhSwv6T{Z2VM!Cq9Pxk!P%TmH|Aw{QuDQU%SpLS-K$lDviG7 z*-h{IWG6sU^hWgVPV}VcJ^A!a{cBfMW?}UKHgI|ZoKp&MQV}DD`Mz*t!~6-A-2Ys2 zMA7F;Tw&x%AJmfqDLqzyKEEE0T@!_cri#LAfA58T}m<2)m1-bJ^m5|SDtgc7qxLj*=sDhBQyvL+8E)delAxFHot ziQF|%gXeMo5>SEq>C!KJ)G=%R66z>P?wC=|Gx29g06Bn=!pp5xD}_12GK7nlRZ zR}n;_VwL9}P4jJAKV!NZ?LF1#LJSTPQiIHQy2^+u?{5^VZ#WN#3)%-uqH0|psS8D`MJIiWZq`7L>|I?9PTa1& zu?2ra=hf9@=8K5RjxxRRz!HUmjx<0yb`2iTGA|Q=z!K%!Gmb{E^1jKe);Ujt zaQ;Z&ApOw&D+OXbOYO1eH}YJRC3(M>&%Md0dl@V$ESl)2-0C|}3D_BqJ$_+_jT;0W z$f;qCx>=x}B;;uWpGzp$ zG_M(nTW{kHlG+1cANzNH$!D>H0t?q=2COLd>OnZ$vD%c4fr?Zx_ea#2ht9p#4y956 zX3ZAHi77gM`N#Y-eK>-m^ET~_(1AQo{=}j;2V5?JrMk+B`&5njJAu{$abt`M>}U&x zdN4t_%(uF#u=!JwZwCWaxDLVhJI@Cf0|_0moIiUguoE_|zz z*+s|4NT(Zwu1M!4_n%LhV>+JuTLKW8+zmu zUVF4`ZnBoN)xfYc4`>nXT~bE#5M2(?x(pvGQGP$E1{y1O5*NsU-yetOpXqdWZqVYNuapl`o}MK1tlxWX(_O^?7P4M z;QlVSyX^4r^`Mz*1swi;ZB#IIQnTGFFPR(kI|gA!iT1lc&y#aWdDTSqnMhD1Dq<#g zVVa4(Zt29P-r(N6BoMb0!O8Xp)>i*(ZTJKNH2Q?c*`Lf{uKdFT@RKu~24qi~@?HXs zk8R(W-V=5S3@b--h-N5qSDq`K8Nd_zovc2?UEl_tOCJh& zs~WX1uh;3oFL)=>{OzW9&j7l(QWYqz4(b z2-TW=TtYNs^S|F@#}9IV*vtGX=bTWT;IRT&ILw1nFXsIc3+;4h&@@LxY(ZEK=)H1T zy0(FQCXNR6!q1n-vx47+NlH!Lm)6JfI-8Z0y zD7E)Dp0*wQX@iOYT8fgk5vYsHE;FJCXDDz4IA>RXv&q5)-d2FdX! z@|OvtS3_a@W8)Ib1GLhF?F(GA&NS4wzQcI4A~+cKCuDx5X2xl{*gucI%&(i+L8awE zM>I*FDrE0kh$|Oh7I2-mq1{Q3EthpBJ!f0d4l?qWONqiR2Z#lxwwb>!%laN{zHaJP zVox;C)VDhcA9xL=W7HWgX8z~JGPtdlo{Fa1>oQw$B%>=6}`4{*Bjazhyw_wgv6T|63gb`(1h*Iuo5) zInlp-k;-1bb-#W`W>6#lTOCw!`K{Y&!w_0b=9lTx-r}fJgD)>d^UrmLkpS zvTr6AhA4rf((Cog)ZdvS52pfhvv@e=0cyYTY4L%HvR5;#Aoq`tHVkUUs9e_o=lraE z^#BwUflZ@jf6v|{zG+Y-(0UZ@*Y=mU`7u2NVP(bnh#yXWLi8jTrH)DH{cu~(csNnl znP%vD4>+<8uw4LV5vh;{9k(67Bs#G@SGy-SjuN`wYP{x>#% zzZ8fww>8TQL7;Zmb=?pcbt!P5~;h#A}fnt$_1`GTPePV^Fj3k7q?UzsUyCj1B5aCER z-o1MS??*7{(*N?e-X3b??*5$;Bl zbnxs`r}6yg$V`9?UI8Nr0x>W#5LX&aiaHOFogj|FR-kymx`Ikiw`^#hEHShT@_Yr%6I=sE|-+5d~fUxMd5yCVKF19|L zvMuB7^I!9Iqc8QLPP1?#5C^=FzpgN!O0d`8MfVQw~;A%tt3u;s4}Wrk z1Nbox!|n7ZMftYx{F&Y8-m7Fozr)LTFggS~uB(=v)jlcfvk> zQ_Y;+X6fsKzkH#S;GbNd$R5msSa6&Wp>du|1v(5E#f9EEet$zIb{iVndsZ3c<0})d zHYEHyDIhd{spJtj)?afvQ%ARcYH0Hf+x0SW)X&m&3H5th@)~wqyq%XF6jJ0*XX~Yk zV6V1X8KP1NIEcmmMcflya+XDb9YFoqVpa4hgco4e{7cSvFH1>Kp9yG4M*9w+W{NjnlbLVf$dLjK{+4?e<0ZhsR~+ zHyC5W_jEMU#8_e~-;iZQ3L)<`p#0b4WR6sqmKo`Kt(-|sHP~rJc@i~=(!DSw?l-?g zbsC=&Mv@RtY3+soQVV72m*j<8#`tNV8Avzx!+K(m@ykhu=J~d)Ti$iueXxYz#g(;$ z*XAl;Vlm|!>PqX77nSaqSKnyAb;L)MKR(Mv%%E7JUT;ckYQ8V)xIlH@0kwpjm%}j< zYgAzhGYV(ttkw9{z)K2i^}hT9CJT#nFW$bd28Ezs9_kN6qU{au01iQ($gWjW4M=^bLv2xdb~aOnD9;8 z--A&`{#PHbT{vHFzcL(wC1O>6FF-){Zhf&~>*}tuJ?ZdZn1gNplC(%b@E@l&m^M;vYV!4s(N%B0a6jx^41kP8!f(F**#DH4cX<8_T>V`>^}~^i z%>4^a{9W_+&;GLu8LQ!k0r`Kq9-Tz2_`f~KzMP2VjJWWRuWB>@j!CUalZ`LKDF1rl zip*5OuuWJX{ObF!_;wH;e1HA>*YZo*{??j@LZoq!$Nd{#=eg=?;(u>L?O4w7#Ll@} zgzBUI`@`!0Ij8?`ZIERpO$6LQW7>tbN(IXNW zTLNu5cqwtLKXsJLr?K7P{l$ftni=)ZjpZj)C9-9u87_3(Z++Z#ArF>U9zct&p4G*g z^+#4-`{O}fkLzgqM;0Dqcehej2bP*G?#(_m`#wyqzw;gC^ZOUCl7?UVee|&Yv|}(V zo-MId)j*mI!1jmnT0YQ(rC(Q?&WTz+i$D6T$*lDd8~^l}V`U$iS+`}2O?5nNd(7L$ z=zOV{AThqkY>#ksz1BZGkbSVy^Oe=|`iqM>(ZTv*__)LHcO0JqfFVGT{N)x5-*4czv}pX~xvD6DYuhJnDA&7lO8ILsO6$lhw_3;CcwWcY>L4*P2&|%s zE8{$Gf;cVH=(QvFv!9<1!u z`Ioowzc$Mc7+0HadMT0cXjPdH^&!I^?1#mwM8Eh${cM_gf?Rq0H(O-%r*nqb5~JY1 z+X-epK1GiEvnro9$^UxAyvjcP&s@uY_a9|t{`T>kFH`()cIsR*^YcaGSNRJ6#m)cX z=Q*Q+5{$h}8K6j})R^B7-wN*6*{qJ^{s9(sxy`vZ*Z=&-UoV~d&i}Xd8uS0_fB($$ z=lb9O`H#HKyS!ijiDK|S!2fvjpQQ+d|4S+OUr+hxKi29g{pag5fB!f4_wpa$UnPHi zpbuI8&(F3F|Fihd`+v-NSr5IjbxA%i|4$V`|E3-&_IHK&-xU&nS4jR{A@z5K^xqU> z(BBpQtGvK}YuP{lG0b&Y_n+4Om*PJy``Ztm;%yzb_4Qdl|NKW=_xb;C5#fJVAN=q7 z4@ds4@b4sy|DA;KzmqWjcM`_`PQv)VN*Mf?cK!2jZvMxwt^eFVbNxx$+*#+kFCXRk z75?L2#nXQtCHwTtKfph){;_}BE&nI{e?SoV)Zwpp|Conk`R5~$KY#y`4(-HoKy!v1?hp#KAcA?*LXhLG6*azp;I`oC+yU!Ta?KWUgCVF*G02Z7 z{T~#XqW>c!X%xyx1fnVUKitQ2`#=86GiA?%{?EGmlsP!|<^MPNEIEPwe`=q7+XtkI z9TV1Hi6;0Ho(*EL<(yI(5#HqT5!f{P8+ zLZ5YwCHaAic<H(-Y?+aN=gvbfhBs=~PW;+fUKL+Hh=6ha>pszvzf*be^7px! zd4dw7{JE#}qS{im&m}&XEc*9nTF-4h`+fb_`zORa(T5pe{(-shHw!td;@N+HW^GL` zSu86NqjdS<4kSs^G+ylC1X~jb=3}^16P%{Uc@nkjHlvaxPi$Y?d!^5-zUQWS>BJ`_ zvEu|LG%O0U*g@w4=+PwG5X1#Ee11}?C6?edegQw~B`vv4g77t48 z-@49e!5&(2!M{A#vF8_?#8tp%^(OrNwESCowO#?xjRYHePB#ghfFP0p(|MlcjHnnv z(rP;Ct>u`PDZT`*$nm>}&E~}DFa#S;04JYR_5xI60w(voEl}jDw|(N5efRAw2M~CS z-F#LTYPbLp)D(i8`yN)Gdee?sfM-MJ(>#MQxr-=FZX4>9JO-sD%br(h?yB7ob&@=g z_xVDaj`{!?)&6YtJejeG)4$|=As$n5hQVMgzlNgkeQyqQM1wrpFnsV^@H{_BCLiM5 z%otS?c&I!~dakFpmD)_P^?ATw-1jJqz{f@jB#DPr`BZzRDBbjxn>@KBe7%}&P`oXe zeq#XGFONxK?Pe)q1!z?_@iq6mdJ%iLgCq=>fcjN35-KKL{!GsKTSxv$x_d;DBIJOd ze#`~TP=%k^F2Bk&vTXn{fm>IA-+uj#-FM0NG9lc-h=-9+^b#TgOH>XkI z+xIT<{B2%KE{Y-;%_s~4)R^nFdE+-qBkj6}m_><0IUW?Fg1>XzmovxS+3012AKJc+ zB3;*$veEs{C%^pjvrFY!f2Ka&&DtcK+#6aU+(nfa0wqf?Xb{h+tcln~gFad*W z7Q!I^%#!2Gi^1KCqdW zXu_kxY5$F9gKv&oke7ruB28O=avBpzOQRuXPO3`#4G72}*u6bG-E7$iT=^>=pCS*{ zHlGv)$1O(!lO)Mt{DUT^wIjb=_vsv09J@SPq6$qAYVFh);5$NzelGwR?=;Z8fRCHJ zi_r7vkVHWmFoHW;-go~e&q0X76HNOVjpH#1E)Xj;xfKQs0uaO}j)8)^PILk#@Gevnh7{f5+MoBKlrt7}9VcKVmeT{3j@4xnKf6l}F@)#)e1pDdmO`eL-+-MDf_?!K{-3$+;0E5)RH0vt0V$n%5$n4Edj|wx;#{(gP=)bDXMQ+c zk4vTj(QZ(Vw>kF?YKKD{|5ANjXqM(`#MFg!2WA2HC6KAqs_}m@8_0unc>~jzy zsLatYN5vbMNk|8-nEk@j-A8|O{RIIhU;v;3e_|ztfK3<;wY}>j5BqaFd-k=pzAm~gZ_gOJoX(8@{ zXpb{=7}}VRy8oW~gf|3DdV=;sAKfXkW!Bgo>D{pl;lw8g8RJoHu|%X!A1|j&#=}vk z1=)I>2w(NP&)Kf#DNbk)>hp%;c=@aeJl(28X91>^Z}hx`KYNR4cN}pr7gGdAyj-F{ zbT43#CdCva&@3V8S2ln#UPo7daF~pLi=~iH*)2=VadvEzU=` zvxD(@M~U?A^?3rSrX;T%16TPlD3&cOrjVRhZ?1~Pn(0+L15!UqBIk0aOy$k9skNtR zQIJ=VuLZ3`eRMbb-m9o#C#W1p#6_Gm1$?s?;#oc`7SdzC%lGAFy}9o@Ow&}f7BB8I z?@ehV7t0Y0(77Y}nG*e%XT}VC?1BLGu8JAB#T3Ah;|2Y4cRum^`qpU3iD%svwmma| zDN^(v5&n!JJ0_c<2HtKmM565}x2g#> zs@Ci%B7l1pQ8+&in3q5HW8p+>d=IBO1r`Zv4)R40^`1)GiTo`H=Lt^83%yH*{to3A znn+bIz)!;|O>LPrLtaMCxNk|74_HVYK$}Djk_K0>eT6>NZF>6gH26#(p)3B5{3Y7{ z>(RLg=ChP8Vn=Bu_zcKJ6*T+yT}qlbj_5QReQ(&1kCyqFJuIOgyiB& zyFHHblKIETPo(^goBf*Gx~}(`dr;^f%r=kGwo7y9d!atMe3$Jrw02)tVdL|K$qQ)} zE33Qs3k9(foJ|nC&7uPC+-@FgtknM9et8KE|I8gh=sjRGmq)y0wkNy3UT^_E{SIbH zyeWxIjG&g<#H>My{VE^eB*qNra75Oz** z-|;+pAPL}~xk@aV}y}yB^E8iA1leqt^)~3rP_8IBvdr z9tq@bDzkX%0HMBGXp^~bIoQMZ@ul}oDjyt5hwrkFa{1}SK?9jP&dEW3CNOvM%kQC<0A z(8juMh^i!CV3dt-!vy~VkVzeU>4CmD2k*JJA!eWKdr`cFJ<%N1*ljjM^*9(3@Ud@aYnOv+ zr!5((4HhO03_5S|Xo2T@>(%nk@0`{MI^SIGw9aX~`HVy>iEY7L^>(6Ggm}G@Qzgwj zbL2I%L{4_ztNJqD@hNA4-NLXB0{CoyyGQQEC_i{C>rghTrcQnhc=gze8oq~=J{9YA z@=v>(&kzN61ozq`U2()?hwZDkaz>$T64!WgC1N(mxwSh)?VhQIqS%CG1jZ)7U2h+d zSq8)$QwNcf6?Qzr`bnfg%-%y}B`KP?GnNpoRY9&qzeOU+c85`|(>UUKc2@BdN^m`u zb%N@F<0dze7`z;-svW#V;xn>8E-ki)R0;m#zj_Gb^*e+wBS0U{f+rCqyM_-AJ_1Oy zO;j~duHP4*@kC9{Jiu5(H{^Fd%s&3;lYO;cmX}k)^@Y1E<+0`td|1NwM^x?BfUqtl zeMXd!?@;&4V_|qaPVYApGu&tDI87EL_{|ECBdS$% zGB8`f_^M=m-+#LRB3?x36^wJuC@9;;UQ4$}xC#VrRBL+Xm%CEi+QuC}$d zOW}34_p>qdvyfIl4b1rdWTe%9yKeQ4pnr7$3o;b;@&{?XHk z@oe5=44$MTBn5JkyhcBKeAu`|OQxYLuIF`Prw81*e6me81yr)J3(XNNDI|gF4O@kS z$D$}0r&|Yu?}2pNOxEiS!yUI-Ql~GXid~1i;XGSrXMH?2!F4)sjAD93j^_s9vXv71 zc7@&~&MI%oM0ve}){p}%)p2qnHb*MxjQ-&I8MGl zvI33`;jqg4SlfdN4D^>YOAbQn&5Qt5 zfTDVy=V^kR_Y??=bXqn#AQRC+)4lwU9M&;|Z1gS~VvbEkfBJ6EBnpgPU zBNp7`k5y-8ieA-pb?M~U_2ybeRim(-e8mEyQ+=y#FtIHD^)$LxtL2tBh98!a2~E3w zO*lbQfj;@dEW(mO5C&(WxGq8=sU=e5;B@%to_l_CykUq$!L*k5{ofM$(T*uge_)@! zDK8G7_9E-}EpFA8-e<#?eB{bHU1Q^yl}?yGxn6YAHz?si#gyI$Vn0CZ(FT9*%57Qk zK3~BwG)=0dVE+B!Z%=$_N;`i$XwA;5VjsmDBp@O0hid9$IiKa?`UKH}8c~7y!8Ev zi7M~9=QD4Jbot}x@ld#m2M7Wye2e0fJ;=`WLw%^a_rPD(`2ef;27SKKFifHLEjg@o zc~P6$z3WRz3c9tb-iUX$bGMkvXYRDVzCoHj?S1*f?H4kH0(nuaA$5<86)8a!_OiRh zL|J4$+P9zv1yAuPThu(CF3UN9pZit9xhyL@v1oq#&bN)paS zgI=f_OR#`{V5?0g(XV?<8;#IgUZ1w#hI*PmNNH-_C*P;)H94=XrzAZmtA-913 zot3QDq%!Sx92+GSo_zE%9o%ah;PIh%0M*JJ*?jkbf5QEqiXYq9KBjH-2=yiW>_g%r z=(<_Zn-Cw+jDCJ~qDAepp^1>}(VSO@%Dnho(Gc8yY&o8um=>QHSR?IpzsK@H-~cQ- zw2@A;hEqpIf#hS6umo?!up>c3)E}n|0DXHmxSoPwFCs=eY%o0{50^~2W5D%=*CkMj zN~RT<;s$=>9^3u?aQ)VWLbqTVg%dFEdnuY22)}ii8sbOQ!8{4u{&qcG7u)yBWI*EC}J)d`?=j^xiD%rn7wUNY7&Q-oGQT$SLTLT4B## zP--+}Ycy(;xOInOrXn}i(@Q1T^+P90ODc?onvR54+k-q2!nq&rU50yq_P^W`voDOdQL~6@*?#h7aST-nkN6OU z`Uc&1-|b2czsoXB?%|Zbu8tq45M6JcuuKSeMK5O|D-RVIn_?Y~Uu4hnwZkk+me&eX zB8~o4+6AGw=uv<1o5RcLfkGlz%^>qTFZz@8*Zbin8xNZvLG2RU>W2-t6@0k%@;=Tl z`OWqh$NCbAJ80V~Xc{nBDI(gDM%tKs?+PsL-hAQ@HOA0ib|P*ZnT)hDHS>gzHcaEX zrAo%|a$(a#%D30la@X-f_|WJVqp?!Z0Q>j!GB5Q(!Z$3^;fl}ej=(6ie2>D)Y*spm zzVIe)IJV`|L-rrG$SMH!P~C_IX!X5yQB5`L9GYM=-Eo?emb>!`3(fD%&vO5vUUkHV zcmxls;cM|stj>ry?FuQTc;54e9is=Tdtd{zCGER=oDsF>=(+a4 z{rFil25W6=AYXf5`?sZH{Fiw?J!Y)K8W2|Q&i4V2Y2oUh)i;x{c)$9os%H!6Tun&$ zD(^y)Z_Cl>N8re_2Wk_lxYpLHvDetWLIC-wU`L<;Qt{?16m zb;0%By8zImPVFn!uIQFm5fC7ZqGIQ#0eiqAKMa%o&v{jqnOK#<8(;7f_2arpzP)_f zZzBPjAffbF!xsq~w}k-W6JFsa%|W|w?QO0U3kk9d>}Ia^oqwON9XW#;y0PpY zjF334XT$a>!7{B42#CCO1|qgp82;P*F7q4DvSwwP3C`t5+2XT}L{YTa1=i4=f1T?! z9P`*gklc~EyOl4D-{Y~=hMicgeTHNB6~eo$U&wEK;n8tEnfta~Tb89O%U>fQCuf_9wNEHf^wmxmWVTVc+SliW0U&&%VG(CLWxO?d zYG)6(?CNTRkDYD#fSoSzedQ2pbsFP?8E&*EfJh{Y+mDZ3M(V5zZ3(*;U^B->0t8O3 zOX#|J$bmP{Uo4xBWmt3LZ=I<-l#1CI4}Z4ca%$>E>QbcSV|6^_5;vaEvjr@`ep`9j z_qNIk5QRs}%4=?df(3rih|rI=&FHs+Q=7Y}uPyZgAbYv2;*c1_7%{e9hyH*y~tLD4R>xnddIh4yk>zG@vsHo(a3$+!v>wbV|?^! z4#SonXrmnHfj0}4frfE@h&v`yZ{D(6_k_L)PN6Rduu|^n-tEy%AoRnETHb+nk7=vH zx+EAigLrJUa96|af(bl8Ko$)WS13=-aGUkc8eBH;V$|l5Z}TZRk$wXz41OWp3i+Gi z+nUFKo(q7h2Obd?ruPLt{Dx62dE^0Pe+u)nQHzNH08sGisR9e`6GyN*G~G|INzt87 zBr9Ef_HUbKMcA(>kV}M_CmEKSY~}*(Zo5WGaIFlBhOCYltmcinWn_0x(-aAfyCcnl z=l2lsNep)heD66Gk#F}g9UIBd-`IF^;NgmQ z0{-j>oTNpRpm{^dGP){&Z>S+ZC5i&neUhl*x=E5$jEX-P|MkCd;p~*h&%jClvMXad zRpjUkPACpTpLmlN30T{-W8N5On(z&jnzhbg7v%XA5RYR?U^J%z188VIlL1OTtO)J$ zhrf?6x#Epj9mWyY{lp<~b%o*Sj618-Un>0gwYVYIw(kMx0*g7^aS{cwh@k?3>KNoZ z?NMc$fvOUC2_PnUooorxrVf_r%e^1vN`YJPtCl{1Ydo9==&K?HCQvn3yZ&16cdEFZ z1!shN5f6`gy@>9&$(DTI3=^Vt-=hR^JjgCiw_plQ)AWn1*!T4ecF|$K_wFm7v9Wnp zt{8*SK#BpuLHMje=hJ}aCD9?*7Gq7;!zrgH^dExynri$Fe$vKyY{>E56EFlxBA{cL z=Qti8I3MhRs+?Sv!Y;(m!j1|P90Xw)$N3u{1YYoUg{nd^AuW4=K5+QuaHFPj1;iZw zILvLFC4?p*eDRvj4QyJJ3lzni2k?T33F5#GqWj$$#MbxytWLvZKe`lciqb}GWhd$M zp?yVhB2;|hjK&Mz_nmVLLtDKG_KK^iR6u-^WkgrDz>x#7R>~M;|>5G z{V%ddf8J}{&X;hbl)K8;_a%)73BDQ8&6ol1Ju%`grez~=O?R>Fck(+kb2&g&U$cWb^3C($)-+M~OVmM) zWGy@n5dB~zH@WmAcS@DvNHU2NdvE@vmH)7M**-Fi+eM12>6MnI+T*!--;~8GZfMtd zTCC00rUd4)S94@foYjA0m91zN{r7vd-Med__-Eu1mA~&m(=+G`OoEwQYVftGyWW=t zhP&u%MI0^0Q{5BR`T{8Yo!qJp{f%SyTi7PZ%9Uq~aFyBrl}a?8hmiZ3Vz+ zcoP+(gmBNTK}|nexq!h4w#&J@**@dGLUqVGG-RHm86FigjaO)OP!f#7Z_v(!q=yK8 z^`V!zotCOiRnv9LCD?bG+qUBKaF$o!PebtP446Yd?Q3L8mF5%Odl&A`n#_o!Ed{@- z(T56<*Gq#6(xHVYgdjfi<{&xqz|KWnM89ijQPVH`USC)A=rxDgg~Sh@%ZX=Lg}C&fcQ`~pB)>)rIV+QO9hyY zL<}2E!wH+oh>MX1@iI3@xuAdNEoBK5oe3pyX9CqXRHaqZl2AytuhRyn2O56ob$Nlca6un8;MsV}K_FTr^S&w#b=#QYY%QwM@X{m6E1G_42nqEVZP^ik)RH}|H&R-&;i zP>rcWd}VU!xIczMEvlk4naFu40d$MYX=&kPd}bb7)g&K2@A8Ya6Gikn=K&Qsd|)ds zP)J@(2wp%`IJx4vKjuPQzGeU6L^G2!w+3rOF-5!$4i^tP1Xx6yG93d?W%{! z9U9UTMW z5riWIqf6t!>r$o%ZQ0Fjk2>rvv*Ea#%voy3;aN?8wrQw3JUCBKv)*LBK5LVE|@PM8Kuu->%Ub)p_qvC8v4b z=IO-dAbCs^m~b!ze`jy6i{B;7)hCZw8S@RUXFRX?;o{VJbQ}yn*gY#~45L7^SuJZ$ zcjizckE&*hlgm%ZU~#5}Al+;p$bqO)R-OZeH#IFr&`&1pKyT}#i-s^j2&vNy4EqKG zTZ;g4J0Yz`=ZzL@2;eE1*s3+t@IE5be7z3-8jjc83Py8K4jfwghmJ{_o zsmy!wnmO&`?3Z-;KEzUNd1-Kz_VJtQNBT0~Y5PWSr?R%@1)cI$c&`$F97IgjTy$!8 zFkPJso?y>&kJ4~;_dM)}JEYY3XbLyLrF`ob&s|n+Owe}&^8IASkaGBsECfFtT={?~~fxrWZ z_>TXYa#ugvYp^W#EHx_fefYAwh+cQP(zDdEZNwg_fKtJ!@cD1%K_EA4Nx0Zc@I(g1 zTLQz^_IDZ=*IT%foxTL7c$0J8hN69ktA*85(MA|XP#NXrb+A6DCgWqy7inOdW{!kF znWx&ta_YL|yJThNaPz*?5}ecRw-Av4Zi*`Y>!q$^=14UkjT12^tKp+{g7eYJ3W2wO zn)mm$IAA9hx2}KZ5A-k~&>0KtfRNB$BjCNx?~K;%*v&3yiZfr`0K^lm|C_Nl*>+TE zvIZZB0iEcE5{SOQY6tSaE0jO&ZrX0s2Pu41{s=aZf#wR$Wj=h(A*j^Z*>qi>yNBE zbO?xK0hYk4 ztaz~R1805s^$6d7Qfv?Hc|qatN5kO@y)KSl?%b?rZ`&q|?!&)pcjZE@*YJ9IHD*V?OoyNKIFrLh2G@-l5+#J+|{N=92DF0Xf=J;ZF^n} zL%{C4H}9t5+GDH4`X~|m;W5N@8B?dPgwH^p?DrNrlX3-brGyqb2+^s323GweSNbCx zU)&<)#DtQsDA5$vw&;*CwA>godqf{khhcaxuF?`jQ3UpO_ck+uVL;?WtvDX;;a`93 zUareRdcm&!9uM7yi!HDgC2j-h%gWz3DFvBv>QW^3%n{m zMLF%`WLKFWh@^Yixhm7#TN-0#?zi9{oxez7B> zGhvUOop@I7*|^B#K_IK>p{S#0Opn3(W0;7h_iEj#skrlCa*db*c61=IX~ur1A_>?C zt2{m%qV!+4*koBIJ6}PQ>7E6?kN^#ZVH|Ud=h3tJn>}&8nxeo^l!%mo=#Sq^^cmo- zo`;K{SES0y{2jo(2HivQy21zFrR%DQ@@(`XHl)_fcl`RPW*KO(dPUU?F<@PG@7}Ml zdJhOHoG*d&-flmQ<(UKewK=1QG)sfMTFvX8E&7tAP+}4nP%?+Jv|7SGi96TI@p{_9 zMeMa3;`dzU6)w}%MX=KB(Z6{tVaBxN4ub$moJ_kfltNO}t|qP6Rq~TysgMVLY0V6- z{r`$pjU`xgYR?Gcyjk1yTpT9B7m?*SM~cuDgrJ!Ia)m8U5VE z0jwo`pZnkWocI}{_sM#zIlii=5HmPFj>{xzhPRu;mF>auT_|W5DKaU_bxotF+EBO* z@7%M0Qk2p@H3u!-F{lfH5v6@T#pIpYHdR5MME68Jue4?SU6NN)5_e$UAv@3~s2m8R}{}7WKXSa+jf)UY9j0m&n=Y!bht0D4DkoqY(UU}4}+%~Owf^pxCz16MJ6rF zFv05M`76@)&?n+ZupB-wGe2HP=EiYLRxTkcIcTGkWI1g|6)@m+zxWq|vs zl6A|@qd^}iZ$}pvjbW(KT<)Arex-T~efu7z6ul$zeCzu@I=iM}`lAv?c+=YY{V$*4 z9C_hov3CGPK)Sz|Me=x4%)>9?O!Zd|2~GqL5-q2dWRC=;Jiy^Dq)Gx6#Nl}J#wJ)U z@}tK>E&Dm*+cp(sl9Cw8=mXG}<~Y@&SIKa1Bf54kx$DWHoL#P zin@`J1YC{Tq#K>$yQK1#%Bn@PaxJv zJg=`n$4&EOMDFeD?)r-d0{FUj`?`0CpRk;6U*C6uGnT*a25fB^P%BZAc6jOhJimcp zPQd@JoK%o6TP6mqMd-=x9{4WIG$z4H(=M}R9@OvAc-_4n%j_7KNFb);@x-hogCL{4Hwa=e@&GYKZz*uO?(WPO!vD^UC>WU(A*=$=c48TmS zT8&DRc}*|y3=0(DVEEm81L~E4!39x3l)TT2Xp~!+h%dLj>yGVsbA)wDBS_jY78CLW z*V~hO`<36JZ@x-&`QEo5BkhE9Mpkombf=dV+~Gw& z^U+31Z5bLo!?{9fN)#p_h$?RT>y!4!IohDIJ9E>oW%WTPx_9a0)!UC4)|R>gflS(K zfBY;%xs-YU7~x)K-_Z0iV6+Al{F)r)D1Q3lBZMt9DRGd|Hg`@Ha37tuc3!p{CBOKWDl%I! z*+YuuQ@7&!yfst!!hH`ph0pN=#@-?i?Z}!=n!p-NOSPhEc*ZaA=);Ta{K{3^JLYX} zGED@CQEwy|I@>Grg}0rT{IQ3YJQ}Bk=y<%M7UoJ+2++ASJMZVdd)GP*sT?}!Rk-?Y z$0H7Vo`ubR>~C5%`l=gk$71P=UzQrhZ<}|r?E1IB^@ z^MaY5S3IBMS&QoEdR=>rECEZJe#pO!y3;NRzt$9DsyZ7O=IqU*@?xpynTby%Dnqk3h8+~#2+-r(I{z0>&g<&`;>n*iSZbhE zWFdzkqj>WxZ9I}s{jpz(6xfRtHAF}nfbaj8uR^P(Hb2&hR!vF0_(O^Ec4^(J&-6Zx+b~bRC{oG4eUJ?a?qB+*+5@~!trz5$coh)+ z4A7f&D!{R=`uk!cUZ1`*w*Wwpl@)#T$YVPr`^Fou%TQTn=jmAX=q0cT{ktR`ZjT4O z=cz8UwJ-uLtWg$T#5B;U*MTDMx44AkMGXGxmwpbHEyL)#>7(1{69pu^uj_{jmLhUI zz}1+{a~f=xNExUu|FBVC`^eGvKi8H4wCL8O)n-<_km-PR8N|~24AS@m6>IwR9y0K+ z&+vv}eq@Z+CU+o-(E|9zvIMK#HL)?Bn+sDog&WW=t;$yu0Y6z%{6q)^Ko~R2c9Pn> zwE~qQG}-fMIV^uZ5sT&5)t+d9SBr;nGtrs$*|xL#@V|!nC#MJ`SB^6xuvKG9(13l}D*)uUlipqZzPXn05i;Ry ztjP@ZrUH`dJ_C+PN3--u=AILLEh&+uID&M0d@Q4?uHTPFNDH+t1Ej*l6+lZ)0sPM> zbZwKzeO(ZDk&3l#H!FU8_4&(}12B**PGZ5D%WpH)02sXIAU8N&(L&Gm=bbq|{WUGZ z@=rddCILRJv!KUKb?0&Jvr27VvH=~^?U5bpZt6%5xupIzw1P~2*sXY}e!p2;6U?eI zt-NSj65v9Z{<9@t-_rafo3qDZDnVVv9pt3jm-h>w2yJjbzi{{(sWB#>>U{>GTN>B0 zf17~pxt^HdR36ER6npXN&sa&u7$q@AUB1)j3tbdML)0qF!lT^3qN6+f0lye*D-_%} zFU1jndq`=+QOpNQ%E#+*;@B{)OrfYMmOO~E!&ANb0fDCv3o%}%k_t05lyM1vB<38% zrJw}}090(>drv8%z~8)|cT}K3)gY%Z5<|}{LrFB934&Vt@A$!5#+eR|xDpIz#Pug< zFxi<&`~1L=TH1E~o$oU$&uxX~6i*pZz%h)ZQnn!QodA+{KdsgB5Gsh-@0Jig`}7R> zXBsL6n z$I(#?>%aZ!>#Zdb;!8@eBmpW#6mf<|M|fts-V0!vUDYN6bmq-;_8jnEwOxBR5_LSBYPnM8mEpDHb4p-8IrcCf7nQ|f}B&a#xl2uV5ltllrZ zrC`mJUi4r&ernQk|3)cdd;0}gS80RqoFe(kDeLS0GDi-_0(DcIj(RHz{`RV)Iqs)6 zI3fmxU~3Ny}5}B2&knTmQ_3%PpXI>(rAj z2a!LR0>qMOh1~Y@g-NlnfAD@0-zr%M9XD(eOD5bjFMiv& zheUtmqPl-EoB-Etz1^SoV6kA)R6jN2xU&VT2NY+aeWf5a&isVQ)$DvOZf#fdLNK6f z|M-1QXkAITBunogT00Za2=>EtOvZ+m&{N1)FIQoR^EiY3;K? zxd})a_{6yNTk^6mZiVe3Q}y~cr(=K+_Me=NEsx!lF`p3_Ah8pg9+@~IG3*Krjcv-Q zXel`O4qv~NJ7FfW@aeMit1@M;&t3_}HgYnE6WQU>%mvHT7z$6x*;2~h)UXu=J@ha! za570!X4b^6Rb%A)A(wGHeU+=}1~y9+75y~Z*-9Ww;)(yJAV0|Q(Z^zeg0OVhG=!P5 zNn7+|--9hhJ)j^iNz>MggJqle5Y)_xY6<~uwdXnfF(5bYUU0{^d=PpkQ*=6zg9iBk zFb_T7k80OHE5D5p!;RO$+Y(Ga8FbRV99bYR1rv3BU;$ac3l%Z%4Fuw#*RRL1-nlI8 zzj9fh*_G3R6d+STR=|v{^h=F-k*W=x&40WjJZIOa#hQ`4b(gp9>I|qri4&TpphK?fh~QIbyo5NXnOvc`OtWzz#MJ9xH~&a% zMNZV2^n=)#;lCLWKJj1u6y^C?@I2<>gu!-idj%p?p(a|XI|QNA^MNzJV}w#@rGdsA zb#fWT@v|5YF4;G+b#KR^3 zvPdS9csk)-9hI?O&-)N&@=|x1lltx?*)A8|4^IYxdvTXKR*0|v(@!I;RK$`18e!^_ zeLRRwgrx#Db)BX$V{Pw>ZZ-Y7%*{RV1D`{?%mfaWpL**%0=qWhL+-pkY>Ih0N9hcO z9c&@P+o+qlb(WN21uVn}BvieyN2%?=S26GklcY(Q#nVZc?3p(NK`eTT*d!!;fx#V6 zkdU~N1j8afy$udLMn2nKl*oQg5Q)N@Vy4uIx6l^`=&wSn3l!Tr?5h|zYwSs0>#MxC z{(xzPYh>^Gc^7%Y8I2sq=`P=D@P`#j+)Reb095t!M6~`Hb&7*t{UMln` zCn@YHo}PuLG2!d)^u6_sC@DmG>v$gX$8bS)7 zVWah<8C^349t9DOjOgC)A&;PxU4Rvj?wKCP4~pSM5kf{ohrm5Lt^I(FaG%34^nIUV zoJ}NLSU39x5HS(oKObN|Rx0PPwbM^Xpzm2lYe~Hr8o@B+$TJ#2!?G|XwCKl>vVlc-*MNu+TWQOqmDpMHZbtogja1FsM)EvXe58RCVc9pn^e z#_iZ)aXTYD`@$B0`Gvls!1p)PQKgrnWWjkJ2kJw#*^VV z*Lgby=sB#(26v)x#x@fN@VW0#FE0%Di^5tyPe6xR^cOnMlZee2kgzx|WO61~dF(In zKpS>lTrZa76g7M^q{H%2zjn5!zmDtT(NXyv92PVTNJ4M&9(n#x+#(Id{ov@c=~(t1 zL5MbXP57lKK}9$qs~h79)7@K)jc=5j8v!?OChX_ocwC6uGTHvnvTLT{dqdUxF;^-! zJf-td>Mc+365<|D>Bl}05q*{menq@kOl8Wlj2cnZ3x6J9P0uu!64b9U3!1R)iN~qh z16qTm?};H&UrZ+Q%K&wjmQ$fOwl?-S z-P}Y}VIoF@?H`f_JOX*ti>Cm`7`qQ zul|~jOrhMt=uZc@yO+0VyplaW&UuV5D6ZWD>Aqixih<+K&iMF`+#&LmG+uPNm zkqjzy5+AZh7@gE#U;2LKGuc8wupg+(Ez36J02&*qNrn4_rRLD{z%9_B*Xcl}sE986 zoEb5VnFE4!-*1vsnK`BmABE49Co@70{XBzFgDj!*L21eNv==_%5~vRbB=%@mDKsoM zl63cPE)R9{k`y5a-(p zk7MT|?>G~eV5!jb*vX|;z}I3>h|#5Gk$kU#r7Tsxy-FhhXG_C>1+?PN@M)od0_Y3G zeBtv47ZeZ@xr&u}{_!W^M0lX&vEb^^cA{oq7HtQ`C7?(s^u4zdgys9us3!eu{pr(e zE>bkZMAWJsOTLc$4&1_IWhD%bx-QP>+uX4hj_@1?vijd3lY5-MFhmML+$K*$w9PO+ zw`sX+C>slYXSYGL@bX>nuZd262h|L4qiq`$9WrltF6dhl5^EwG?$$kBVMs|kpSu6K z_f*H35R0$p)Z8`1H&+ePPeSh+V$hn^D>d6jm|qWLHeQRQ4A$44Y2^zvW39&dCdS2w z&ZKMCX@>JQ8rq_#)>2Ap1IccBQ1!)3Dr4xF;$Skw;fhMg!sMI$e&GRw^yc!qcE*k$ z3Jl_xZ_lmN^ZJhj&L%tKw4S;U{ARiten;v0r=eTpGVNa7muTjM4AAWQftF`)`t~3= zS_3dNQuh7hWY9vJ2^7Cyyky<|GVC~O&d*(}$IFFbzaC@ho-d#jZ7`&^Y;$?YHjnpc zC)v;Lwcl)

f$mYrZHfm3uh*4KY!7Qt|< zA$Lpv1aavcK%}&aSvnzOQZFb#ZJ(iXX25SBm46Pd%{2HU8ur75qpw!a8hk-kmKJ;& zVeS6K%6Y&b`lycIQ5dV~bp@)3>WUD;G!J^zJc8k_$&dfk z=NNlVGgNk2M!oDCJ2$kQUD?*#;cXWY^hD~P|IR5A@~$@Iq?$=RN%{vz^FJE>Ifu~I z4K}Pu{2L_|E@M%F6d+?4tCUn~hIsY2Bazb)PaoJUTZrB;7f<+A!g`^YRZgwdhH*;( z9tgn!4g_yjg#R$@dh~PO!6_cIlhd#B`T+Z|I5i?d(kDoL?{p9IJtPnTTBoQB_&s^3Nra@-xWOj zlb~l(AxoXjI{Z-2cq3H}68#`FTOIY^`trqb^Ufd78kyL=Gf&6_dx2jbUA7^rfz8j zJ6oTHG`}||L@^fd$y$|y_pd=-+S{Az%>AvJD~(U-?N#y2w#wV}JO?G%PFRlDoejG>f+wJ1q5ymZp* zz2v7Pm<`0+-~3(e6^l~iy);%M=0M+VGtp^p4vi19Q2B7A9>`4pb%Sp9(VWo1c1g~1ZrB%?_@sx2DB{zTh2j32yOd67T zzSqs zkEp%gZtZV;IV6;aD_Z|TeI7f6UJ}>6bHp_rYT{4a3WH!BsjBo`?*Jk;0N#87*T57EWxu zt}71`J@}p~uWl;3Ad0+wQ8M3wAb^QDPM2}t_cULZjSR$I2jEv2m#NC12Q+Uk-yVI- zSl4~v3$(|tS!Eaos1sA^7T9A;^>5D!ueJ3EgjNvjjO3bbNxU8^|FvWbsv)EFOH_m_ zL;IT-+&4rn)OPt3IfDL%%sg>AJ(!9xm+DSk)WFYzF=9$ktoyCYe5%Z{?mNPhD`ADq zt&nxnW`I(d$kiacZ48mXr4sJvZ`&(t+@5?1e`Ds5Td$*G`1gBIu7ai;LNfPE-)je$ zv%E8e1Zo6c{_=eBj(hE^yS`t-ywAH^{RLe7sun=;J(c5qZ?d#@Ozas zVY_5z2pFkJ;9jXCAvW7!!@7rp6Ub-q$S`?QMv}BwLM$tCH}s`T_XwvuErB3 z$~Pco3U*YFaxY1{P&r6xn$oK6mo+TEm5O5Xcb0&Qlvd@HDfChey)Ewjy*|D(vWvY! znLJ*+7<19_d%AZ`mQQSe|D*UxIy|105e{l}j60`_t)>I3fnU5A^Z=TKNr>V=2ihzI zHXGOh0sU8v`Y)H$YL{fGGApz98DG*9aVow7_#u2>hTFM!Qxa=@{(8eWn$l|U`L|XY zrl~>><5m1KQ%58iJG!6@5m19|ht#rW}(9?Y{e^;4|#2`0Pxoh-71qH!ohPw!uq}x@S8f z{s6fEE9q9)4ncJr7JP^@v}6S=YpETZ3AA?xG@n1~n20MLDilg{IImCa0&LtCFLQiA zmyYn~dq{?gNcq>Rh2~j*m(M%0C6Omax#VA8exO)ptJrrP({4MUIi2&>YEl1mkAGTo z7590`mcsFp5Lpq~yzi-|sMw)P*50{IO>0@V-M5(~9}h{^=2siubISW?$Kv;pa!;Hc z-{B%d=4^!<@3mpppAtNu&0Df=?6+JFg%>Vqz8^0L&L#s_o>}78RlC0m7>0OO`cz1W zp8Dh)DqT?&j!j8UApoNC3C+g-(6+L?pH?rIrOES5)|+LkTtF9>aR1uA5QhB%0C#He zZkDGExCeK04?W^ERjQxty$3~(o>7GiN>c#-$3S0m{nZ!r$v1+CA7M*%(pS`Wg4Q&1 zIbayl%H=*!F!dV8ZsF?+7|4S#7=|B;_v6BF+%|tp@H;P$d7aNN$JPGLbvI}N#4N^B z->*mgln!8X0TA!$)3P;IeM-Z^ld@p)&)Ca60*~*r^RlaI<`Egpj_0Jhghw@GT-Uwe zytB?%{&bC(R>Y{hZCgKxh@oTzl9i=z=UL|Yup~ZfmYq!o>Yfp!j;xL6Vfo&h;EO%!#acQ3G`X{m2;3Z!8mtlN|h(3#L z^{$4u*8xMcRA!y`url|=&y%EL?Gt(L#N!h4`FCca?ArPl*>%hq;JeoMHc(oS7NrZu zH@6dMkTDo6nMfQWP@LoAwJanm?7%1d^8swAsY|Y!(dx(aJwkpQ>SLE%i+KO`+TStT z<#zsq)qnGsH*9gC;Y6nd!z|$YT6k~0Es4jIgvLVx(T#9a#GO9Z18}yDvN`JRkNeVn z!~`sOY|hB(TwVzE)7T-BBpGWjcR@`}bndKg$E0oB_U3*c(2d5`5i!nPQ$FdXF`YU& zNMybk0DTM1K+I~!i}3(4>1t$U0Unt|#vXber3+QyDU#AhX!UB^kuk@M$ipy=W52@+ z&+-EU1Hqbjk*JgY5005mkN&+cMKVVL2(BMl*N=z^_ZTpj1xq2y{IHuj_NTjIGgLU<|OasqL@nNn3%G^eOE2){d6M<$eI0?zzc z{&zpo{`uI03>-e(7dB9PEOC7P&`@9Qz32ilqyE#FT$CwwVzQ>?-CAP&0Ol@&sP*#t zrWPI?990kRZTYb!dwo@POr_N+14tE$E2zl(%z@u?a8i9LN%RSBq-oaeG>syP@?4f> z%xiZ@*;5Buh@^9riI5wQWOc^lDf=v{SDYh{r0odC{K|Ho|*dooGSZbU&MXp*0yHfL6jSbHE#WE zKuWFObNrLQs>iWuoYI4VCu{I97Nt4#=~`^hOQIjm(nd~Fq)YLzg(c1{Ben0()bG!v zjX{#{Aq*1FZ}xF{c5l^CU>BX3vO}5c(!%)fJoGZ{c~&35H?d+2iZKjfnDpTgkWFYD zCPr!D02{EoEtMN)pW_1Vg6Yp^SB~E$;DB_}2uAKOM8OcNk8!QP&KA!A?hbI7i6CgcPP)hhkt{2!ZK8PU}SY`?gw?)dL0@GR=93_=w6H z-Hr!}$bM_EcV*<-@7GUcMZ7^>| z*xM~jZ2_bA$?pNl0l^KTi5>7Qn&ETTtR+^5>x*ec62wGF23nG(86=M;*koAV5>TGc zSTy@`tea(uVaHD%dw7Pp%LaE{KTC~-O;}zfrD?8{EIr9kBfc_?UV1o&DsZRdd4bML@_QKEl(QN2`SZ8+{dujl!pp2wKW=2E+F;I(yJ@xM?d>U&ZM)-;Gx#?9TBKrqn7zt`8uAV^j*Tha3f+hJ# z$Pa;@N3b~%BMzAPK}U^DcLwmMt_K1QA2+-RL=rBdzdFO-Udg7x zkr9RXbsWWd2WoSXDb$HXK0EH=>I8zo(DJr&+E_m?WL>+QDJDJIzh3YyzDnVhZ$+CF z3d2RybhY;@sfa$5nimL#;_~xUzH`zaQeEK?)vxC_2liOrdvA6U!?0#?33$Eh9Hs9k zp-9E_!-18sNerZT&8+_nTH(ch5=B#J4}LrY=bcE7v=5cL3iK&@b=(RKRZ2iOzA=BE z4?`aHzuK&TX_|u#gc?%PwW@(umVH zhG)SC*6(%WikV{DdtoTPUW<)>Sl;$2b287o#9OgD(fDOQ829J+rhm+4v0PE6d*lYF z%zKURpL?H@=z*~@c!t-Y|ZTYpl1Z1}+CR$k|b2eOmt zGSY)tpN+A6jL-!<+g|@lOFKy>tw32~LBQG9rc?O2#S3QE45~hJ z=F>yJU%QgLZ!Rux;xfmC7Zh2>*>->RtdE&CPY7t?pE)fkcM41PeLS!OrbIZ|ui_{* z@1wY_Oflc{i(-TVKMFfo&npWALLy5DL2FC(EOd=pblxKy%^P>gz%z}`>6#~0xeR$p zPc&h2?9w)5j!uND2h?@brWy8VS)nBkOW0blL`RG*ppvdbVgsh1<~H-z=YiF1T@ywN>-T~AI zh>Nc=f~Ke~n>(*EK1L{la1RT>&k4Hz(9->9f8$Hw9capEn(-MFf*_nv=vYcTim8wF zD_}v6^COUZpEuh64jtXobNWbci9g>3=atldud}UtulOyX1FDQ(y{E z7-tX5UMX3gn4m3a0sN~)PNJ*r@gcs&Oz{>zAf(oF4f~bjbmdf+<%{@sT$ZBb{wp(u(4BQQOZTsB#@ ze`F5)7}GMSpLvcllQpQis#aQdvNXeZzZiNp%IjsR`bONn@s(89Fl+$9dUV&h!I~^= z-pe~P@9AwG)juB^qKZH43!UutMQ}of!;@dXOYiTWp+$y?MA@LnXCz|l(aj;)Y+1-L z>a>Ph1i0i1k$7@_cUYWBSLZ9oavhS$0DlKmrcz|ZSW5TV*8~O0Dn!a6wK5PyQY&HX zGdDgSk_K*f@Z>aw+`E5Sg7b&dsE=2pnDpG77tEUw_vqJxo*m8ai>x$jwuT1o%eAGt zGKJ2$us34_hphYOcfpaZ6C@Nl6WJSLoZF^reggR9)C#`a;hctvK98yd5m~yn5_hz+ z?8mcuH9!gn`lI<^lK}6ZXd9iw{4hgf|e?v%qGbhxSfK_q?3>Rf?>oY3NSrV=x zt*ADt#r|;HW$lPJeNu(BabEz@mM_68W%{)2% z@;pq2A3qcJsME!S&i*mM$ncCQV(v$yQknns@>Z*3o$T5}QDz2GFN$7q!D1yakoSD< z8WhgIN`A>=wlRGOU!2^(8RqUyXyqu*9TT$)9+hCpV^D%1&^+{espxmgG2f=3nA>C% zG~_{%ro3Ah3RxF(+7yVSc-dg7WBLxtS?thaEyth>*pr?wm|N9%=Jzzm*Sma=*}cw( zSlJ--2(AH9VL_Lqzv~xJ#FmwWCH?eEJ4}|<#vKR79#LWJ^at*e0)*zp2}b&yrWx0f z+(U>O{Ki_L* zHy@^ol&T#1g^;h%xUs|kJG*i4JnWDidcoMM5n-0KWpvvpNB^+zQu5euHg9h<1VKny z$roYj`{tBG0J-SWDkrajC@P|3p_FcKGf*~Yc1#+{-Tz`@H1nm-ADrCH1EqS8)ujCe z2@j;I^Hur}_ZvYFAqlLetiO87LH(#`6l~%WPG^x5Ud5nX_AKA&oVkyn`y8eC_?q^6 znm4to7PD!7yYdn+YUbFjlS`v$TE={k$s5Ozk6juT$aPjc98-?;b29%?Pi5zl= z{Ji|F{be+K%Br_XBgCx@0FEX=hVu&uT9`R#GPRq1QN1nuN+AJ0Ga4?xArzoXeqUXL zI{)}~30Q%9|K;q05z|Q;dTDbp{g!kJz81|M0*ByEv(fe4UO?KgZ(Z6>o;l-Z4ix0Y zK?EiVS3pX@SwcB$`$78EXT8STvY$5MnK(TcK(m(KK&U-LuQr${fsZzu4@7H8AD@z# zem?}i1fm{oc3=*MrZwR!TjEx!BI$kZ)BT9=5e4&$N8_m=^21>_;Xx&GUy{lI;7>d{ zXixWL4gxRCX{ME-uki)UIgQJ$hX#g%7G^~`3OeB46NaQ|hfiWBO|c}Im+wkX0XbCy z)HU-b$~PZc&!0}Bn;wj)!cZbs^|E*aPKwG$;pCjUWp8G~+{Z3e@tFY+bYIw6pYA1M|HJzF zPe1CVp!OSC#=_mxY?_o%Q;ZeDD2^Ro&OGQO(gMxvKXUa$pYHrgb=M+mNYn5D9%~VE zhy2yWrj=gQ(-beaEKS*XWXupU{8Oyrr2V>kwh7GlYQ;UGG#=3};~TljI8c+9 z@GBwOqjmLT5guOIH^j$xDtXO%1G#OzcEo}2F?6;B+l-s}q&kkVLuX?{GR^jM;uOk;H4PB1HxV-RPdo?q@IGw(SawW-HuALwlPu z#pLP)+z7l^{fX&VvgFx%zOJ|89`BnDpf?`RC`m5c3N9SlGq)c1z)RbS*XzT|=GCT3P%$kn0MsbQ8@qKR@_1_M$&X`IS%!Ur>dT|Qz5i{8E zuUK~X=@sf{tv%;v_T9aSj%k>PHa85!`8%M)Eg`3-$ZS?&@rHRvuss{!_VIs!Xu0Gc zKPaub9IG{HvrKk>I<_tIyn7k4!gu>ZHVqM(5b@xi!RF0vB98ZLN(Ti0qBNjkDG5X{Y`$PZPekmjoA`KFlJ zCuW>`D(2*Ofg>!4wB8&JXpfyx4;n`H4Y|n|$Sg#C+RMwQA|xakm`hi0b_;$5YZ-(Y z`9rDk9T*IRgB2>v0K|=>=xMD?Y)ARAgxLoUj{pbq^&0Ah8jaB*zVwRfn&xHiC6sYg z>eNXQ+lE1MU3JG1LpptD=nrDbv`%0>2w?rs6!Q2D$!Ahdb!9STs!fWdxpA)uVg3>< zP$~Qaq)(Vg-};akfh5wK=3zKo2MCV3q@BQPe#tSG(hINM#J{%sXY73xkc{@g48Vh-L~Y<^Pc~@G~FI>F7sZNG}%&q ze%0?sDbP@jQT7SL=@iRZ+w^0K(A17@o*{pFqICC+ICxYfvQWs2HOgh%E zBO9lN#4huGR*6$merxyD?DkucayzSMB9v;6Y2@FYXsk_Pufg0?cevnF7f{{(T>(2% zC+%B{hA9XncIu=9`cCe)2Nw8JO*FLL<zr zOWSysB}COd8CVl|(WSsanF8Cc=W+ZJA2yxCgEpXXRxTpHX(wE`?~nq&(paj_Rsroh zaA*f8-J*3e7=+%@H|q0u6BU35H}kIBmPKgw>*`_uxGHZbrcug%jW;vpy9-YpB!OIa zX){d*8Up~01HMV-6KV!fwFeV%98djITkkdXJ9$&A;Cv-<5)UspDr`1CZi_da_A0h; zDME~|Ipj;Yc==P$_E!bRtBdU92bWoYzQMlg`xbA@AL!i(a>NVMhVVVbvARP}9X(?*j062rg8Z7zgK3akiLem#i}0P4m7FmJezkOVm-HSe=|=X=)-@Eb&sd zn(Me)^n@JT>$DJA#sM{=woyB?1h1iG+X6G1+W9XBY#h=|InP&HRLjklB-qMmI8L$Jd~*H0 zB619PRS&W*3Pmmc>d?&>;h1jnrgzs;nnv1;lr$$u;hKJ4*EE}+^YxjAM7TAjvsEZ0 zaG|RIiS@T1%h>D|e}0dIPNuX+Kl|Ky>&jE{ev5DVYqFe!yBWXi3Mxc4l`eDceDO)f z#Bc`?#RocpZgk2C@#$MgcE=*O4)CRFa^4P}YOY5Sx1M$2cHMgi`3CEZ{EQ@bKOp%s z0HQ672DopgX|)WW@hxjk^Yda4ADzWJ5avJyPq6V4s8%rx{g#2!9bhu@rWDPMI3q=0 zaXaOs5#&Gp+ACjOc^;S&(zW-&aRBjVvPI+uqn7(Q0uI0SR~G*?;d1@v`BeA3`>D9s z%{}@qZat7EOqzvXS`*DDhg#<&%3}EnDRzee?C_qi(KI;z3yBnNq(&8@t_%GkUmARs zK$x@HP{v#s1hw+{J!nc!Ktm*tGiAsSQ01jH)NU;Ho- z6VU36F%C)kI0Nb;XrSFh#9A9hxczfJ(DkOQC*SY|jiA_s5u|2l)uPj+#O}|b!LG63 zuk9-Tao?`PHoLO9lZXq$aEVT8aQBn>0smD7xR(6JNyn_?oPRb@pXyo9(WO zFx)C|tTSZ6JoU>kUV86pQ`teV*x(L0OciOq>r|(N>Lv1x>WN?7;>j7~uw^(G59PfY z-S_)b*WTsBytfklUmj|Gwb%FksyQoIMI zj?N8dC39lK&rz7{z$DLg5L%B1n{A!p6lU8HZ!Vkc01tXGjWF=BZcBhhnf7(4+&hGD8KctLtX~p`$Vwb0HjUYX)>Qi zQ<^}rtwA$f}*A)Ny+^E^garC`)&eLWE zMf6HBZK2ifwdl@H*i5-lsIU`jqiw*-o0#szAf z3LgiIrmELlu<~CvQ*Cns0b4zS$6PAlc7N%rq7__C|z z!dCPzKX(SCDRr$`oTghD#i}Pun;LjfGga0khERDv-npd<{La68memwIS$k@mkm-Y@ z;8Pb8Vy7gxz4xldxzrwOt|MJ;Y z4BULqanAHNWoKfYl1@onP-?ab3t+G;7F6Z0Sc}sXyrJ=eyM|GC3xscUD4v8)N-O`@ zpBQ>LJCP~a<-uxGneRs>gEw@?@1lLh%nS`4Ose}JYt7b@GMcX&EkxsMbd_&>G zqf2FPO{90h$Ksbc@bX6sV;{zWNV@7(1^v=FxfePB8(qtboa7qbUlQ|I|Mtf z3;J3&kHX;tpw?+Tu~gaosU*l0R{BMM)FnqSFf`ub3>@#I+diPs?@JTIc6g7!@6Dj< z_b0|5znf^hl1z85Z*wi^J#CSHdi;l@i2E*a`a?3pnk`zE1mea?F4{ek_lDn8RL!h$OU`>&!J5UJ5#D|dt`qzF zS6hYa-XsUCQbrg3E5yk0J)o56768e{oe7jzrz`prS<6h>tTZ>q`Y{mlwD_+skCR*c zsY!x|6@8_@*S+Qgt%?`X_%goWxJBw;V7<(Z^)qo^IHaYEOxdwKzH^wb^_ORbhqM=r z_Zl?6!FlD;|8TbglBRl_9)H1Df}kcedJ_~?P!g$iygtkC9c7B2$S;hLKe$bozAUwo zc#|Uaj)8gro2T@&k#vv*!g}K#pPTB8vh~!E-s=~Jp;Fw%+!^zi@+yqeM~9`PrlRhK z!P$Gg@IGpPk>EWXYM6ShevB3}K6N25I%?H0)JKN`07XE$zn9cP20V%(0p7R}-84y* z3ik1?Zlxpw^si$Cd`QAfrhnbJy4RwQKtxiNu*)g)izjn8HO=v+2{LA~$;^l#9RR-i z@qF`+^DACLLx_>>ifv2tclPq+j*F8LQqbcI%Iat<8PgrPs;ylVPVS-Ba|Gc`qY&?g zX}8zSi-%gDK4HOpezf4p0-1Ie!-r z1oFz@tN$UT!lN}4t|xzS zGDMy|D)B8m?@b3oyv09(4^2H)$oM^?OZ`=^y0=%_Yo5IW_4A>9v1I=GXUn`qWg*6{ zcJ?xXJQ``v?MZA{1qA~prCd81(jf%`(F@>iv2hrInBOQk*-G^4Huoa!g6AZS(<$rb zf$6Q^Smp=JVnQwnPXr0lpj&T7eoUfORPl$fihefhu+-Jj9E1ygYjso5vAy4+`b0qY z#nm(a!b!>>B6WfeQk6Va@jxG*!~3Iez0=Sn`P8YdleImy*;AxSg6rnct24*zyS_39 zO}8U>`X~q-iF83w`|D*Swux5i;?P{I?k-LNyG_mu9LO8Y|K$=wQ2luFjsZT+2WLJ7 zzpnfW@XVzid>Tr?RA1f1!qFs79ftnxNh%wf4{Nk08XYYUOj|jsJ`DlT zoexG&U+*Yf*3BlUhvPL8n!9TOl=vM)#FkI0y44Xs4M^nreNiW#!Jvzn%BwyJ~N?_r9{<2~`tzKM9rkrodtmw==6Qcf%W!Y5QJBq#;^#tio-@4{ z{&l(4yx`Up{1a39sTXwZbOtg7u%rx;C>gs`*DLDr`YWS!E~dECQMs|?dopArz>l}^ zN~v+gP$zhi6kNG#$ZmTC;uxXtBnzN?h*9YdtIpHAr^GCCG8!ygenA(;^**c$s(A-=S$B2til2NdI7U&l+*bkpBu}p?I8}8WgIx6lfU0Aodg*jry%L)(BM1s72VAT7?M=b=0Mp;f7u2$yYD2(RbM3y5CZlEXJy?JJr;+2>k)?r zJoYu%8AzbmQkAP2;3ebdW!lCz4J3>3_b?Ec@qwPXgG+WbWX8xUWmEu8@Bs2}J0T~CCpXP9%a_H~0kq163}F=5 zb_Sk434VeEN~FrCig^B*%}Ls$fR$RRJr}Q<%H~ZH;vO`I#t#|TM+t!WeBfXJNQq-8 zU%dxB@xWWFOfi`0{YbV}3CR2O>@~xxR+a}I1(_zxulD1Bu_%A2EZ+zD@q4g#LVWxl z069;EeH$`3-9|H7`Or!6U~b=$=7BXAL&RL0%Q8nkumQF5VKgV#{Sl%z3BadmwuWUF zGwH`u8;IY$W8Vv$TFY7im4clJfX=aXrCgU#lFwU*=iqf5NPbJw zKF3P3Syl$2eED^@V4FvMjA0art_zTBkU&u!@)8mge0{*8aAaV2*M;vd9~?cL(dr!) zI2*&jp#{jN=5i{X+1>5S$MO)`RTKzvfCI>E|4<8ICCI+H*oPU*m7o#@$+a&htVUgv z^c`Cg>A-=>T*gr}UH>eHM36=wq)vo#(J;R`Bn-mQ=z4xM0*>aOj5Q6JD~zxA9`q2) zAl^Bq+4rgOye_hw<{{YgYG+|T-G__mS`d;xD_*`-DK%5a#5WFH~A)h>mjJnC63t0qhG|^2PYENF+XU3{WJ+JdQWRV{^HQSAT9{Z zEbLICf}R-I^d{}Y-yP}3$M3PlfEESBw|@?uq|@^*&tV>y;;Buy2))dYr|Kg=j2m|g zg{dR*Q7C_m6>UpVy%@$UfcU#M%3?XG*_Dr>k;-c$2IRjF8Cx#rx%7zCQ3^homa`xs zWlhwa!WA4q;(N76#AVE``H=yN??vvB9F#Ft3lhKDZT{MR=>N!ghmLlH4bra^_D7|i zk%{0}e9o%~B*@+r1(!)L?^H@=gal)${t=_`mJv>2?fbA3Tw8gK-@}xX64b5_2yjK4 z;~?rG%+}4Xy#iHFxt8JjBrl&X!&|Q{z8uxx-5a@-xZg6_fOO_rr~dgPDwWvFu%`UW zO%76M7?6roICG+KYk;xW5G{t!F5|%Sqb_Vtl*g(>*PB1fjA@2HriKqU!-@B`Mf(+l@iv>gv>_G zUL5*7(gX(GnbS>Nm2pAtr)wcxsfg{Hcu&IVeqyV7I24GYI}*i1#oK_f(m_=kKIIh&^I*N&;VWs!x|-K3)m~ z{5kK-uIn1!W+2e|K3zq=PWw_VTqh>=C-_xT@DpM}`*=TQx^NTJRQeDt*~D?2rP*$q zCZ@N$D<390n+Il&NBbd*C|Pr`5NdZ7ZN>9GS6QvBE;VD4kuAILds)HPsQ>3Jr|!+{ zJE`AJ-i7!%-kzWIn~~oEM=%>~niU0y6-k1HM_F}x)Gs9P;a~3sICa?)_rT556d_+S z__3*kF`I(%)l|&GIT|L_pOTGL<#KSBd_RBX>8&*p59o5Q|2T|fG63Ws?-D$Le;&4w z9Tf_EdOAZR#CO4;*kNANve+Sd&3iLBwyr89@anAlImBZ#k_@$#0_!ih6Z)?oPa**L z(P56`z|rcbal%?&%Wut}2+W2WeK%2=T)pRbk)(ulX?h1JVL=>WXeAe9ejFGvHfA8^GTP1JH z6V4$+XCu}OH6^UZi~8$Zhi2^n@^W}o77&Ubya3(DQA(AP>9GftApSHp>`&t3lH!mH(-XWBtsccvW z<=sS2THos&h5?qTn<6HwR7)M(TC1Nnv+n15qIv+IlpPT)PWL(&av$d9j!_?<%B;89 zVrrPEihvJN{V@=fa&JaO^I)jG6zQ+OcH~&H`Ek^p8XrR^P@SrfH966k-o67;9D|>> zOBF$qFg@@>$k$r>K7)T}Nyd~CGNZT1^!m6z&WtC-RQQq&tOFQN{XO3nqPWrAz``t? zMM@0VH-v5dWU;k>Lt8TjKL5eqG(G+K{QSM3W#!4GZqRa(8- znMg=OV^H0%Z*fEmd4C%lMf-Jksg5?I8q7X{o8tz0Yjc}~z~citY@a-RtY3KbT1XIQ z;4RZM4WBU*$WaF(eoh5^`WBSud?R=^NLplm)~JRWw+R(ZdF5~$#WnG5IfmlbZP8S9JwG~ zX};W|xfMv+BVr~A1zZa9N`ssNWGBi=YA(JVfsxgL@kUcQMz#bSd;c)-_=yqKjQcPr zvpJPMoyuL?hik;e#RlL+*uwA&P^0hv@TH-PqrjRBQk=u2)kIjM56Jk6pN45srT+QH zUroJM=Gf%>n0G68{mF%yNfvLSJ^!}KYuMmOj*l11|5z0LyyQdrEG+2(0FXtS_nu+l z*UQNJsLxhw|G&Leb2wB+0WoZfGP5a50q0QJr#)#u_UQ5XB?$VoAqi1DBEOwf8ax){ zg$~H@G;NzyA;2Mn=8FP`c7Q31099|kx$yR?eX>EKt%pZ)wSi2Y5nj`;R0OEhw6~iT zXdkQz^85YQ`+p%IjjH@S&{6+WpzT?+8%LCr>ORf{bu6x++>HR7i_rIe&PQYwk|YM8&xzs7#B! z3a>^F>EDxEyXHc;eJ`=))dT7*(iz0O0LOgz)B^~*=X`96P3xiY_DdunhY zM1i%^2GJq>^ECta`_(qXcI3nlv$a5WAHM_(Vl(;JLKeM|b$G2^x5BBWy@gz0Xvq4>}q2_ue1a0mr(&yFz zrl&Sk1!%aHlQv?f7tn!#6Tx92;wf@{rHT4ouL;9%J{;8yiLjL-7JvJ#u`xqy$@z^M z`7sF4*@nQWH^V#Ks**L!ic|cn!-hQj=CO5O-;27+(vGnKTCh3O_R>mELR|l8F6B~Zxcb4VRvUHGUdQQfr-r%U zcSkoWDcBHPd7An9PL#zxu~!s()Xt=+5QqJ7}J-X zywLFIk6YGc8RErw#E{MuQiNWBuc4@wQF-07n*q;$G0(<=ZJUg46>oCRS+YM@EoU)w z=Kdw5lzaO7{a7ankgj}9)|*R{0L@g>vhS=){c_%G(%gROsXYk>AtKe&hAPj}#se{f zTkdHAP8k5|c^rtCrginb3a5UjkB|HK&Zx5X%@OTwMMfK)iuyi_`pIu``RJ)hnY`X@ ze^EZyWc=j%?<6J9+8tmp#1iHgoQQ7-aWh7jYo}eGgaKXVzJv8;&$M$YSl#abK&f$# zc_HjFgV}3#rlBj4H3z#2dXqk!2ADUP+#2Ehfj$8t5m4qTdOZGo3o%CM^!QYOlC8=r zT2S5`d`5*Bk%(TDk(a*|`1EH=6R`7PJf^_bJbc)n=~$I;SN{O5J-RokYQ5|P)f*?x zH7ehK=Q9r{^YAmab&2=V;$-ugjy7A1!A54b;7+>Gi$!~s@13rx_M1Z|HPJ~)Z!f84PZ`m@V=aq z`M>wKk_=^&17}PLw>#5C5)F?I-$%e?u9MHo@Cq@eo}S(~PG)=Kss8`>tD%Ea(WEzv zIST{V*YQE8!|cJX-wH$Gl#Vao|Mr`S>C64wZ`P*aMGoNUYRf#o4~(5T9rw5WlFf{Xb zel$6%vsgt86XpmJ-)q{ulW;x)gXt)6ybFC#Vbh-L0h*n0@dCg}GjFCFN35+1-uD@{|n&QXfVU-AzEUmz&zwYn+u_iWc-{yb) z8aLa?armcPO6AZ6ZSRm@PPkR-1MUvz?^V9(xzko!n{Rz}Ps3}Q>OYO(I}@Wi2I2pL ziB=&mYU*xKpbhJb8>XIfU(o7nzAJ7`7HSi;{rP&qQF3{cl{wTnu_k;k1~HN1R7YXk z2R_CZ2X3+fYt*_kojf`+KGnSGcna1F!$xZT@SCY9TsNh6ofj}|vI9I=(2S1@PdQDc znSA2l=k-W~-ZvVMt-W~@ui^EL0S<0YoGRRkMj7eP!O^hm0X9yt@Jg5n`>$4@R!0x~tFzdz_%dfGk9p~V$GnJ>Uo@}yPYH;2j=z9pk4zqMNNGg{)3=X_yF;ybLNR}ok z10>IH*7xr`(dF&7i%iZmZM7MkE&5tp*<9kQABC~HIYRUSP8?LW>lKeZa3Ki6a4$|k zx@wjhM7cgF5Y+_Q=3f5Kt2uATGHLu-uBbGUF9Flg57PPau(e52BTnvN z+$kOC;+Mmb?TQ~(yH;#*r5XA=@-DZCK>|oM@6ehCPk>~N?&Q7u(o^^wR36RrjTtW< zpZ3_MI9=S6tEA4eDbaWAe>`hr)}B`=YYTli2*`VK11aMJ0rnxgnR4LGaO3P@HR?~( zi_MSIlOCagRXP3ICoH1bFB!)(uOfY2!Jil9c2J5FV{84QTPjHJZZ&UBalpr$eNf;Tm4#%0S`+0Ay!E)~) zNkbEU2YRjT1jF|uvCa424BU}E-}dDCohz|<7@WFj4s4j!#y(n>j^ZDeS@R&Csn-R7 zeWXWy^~q}=Lo^-2@ZPQf`hMh8jG*;9iBlOOFLh$6oY!8OE#mPH?NQWEK{X}{KMiy> z>oFkMVNwO>u(2q>Rk>^rEXTIt7aFL@d%)rj{L?vGSVbq(XJoeljP<*I|Qj1_{x~oPvF=6lv1&Cb^cU} z<|C0BuvwiVUl_V|Y^K%$b87Z1lcigSWa{tj1Y&21(G|a$x$rR;#H7W--+O*B0P^P0 zyT2hEdoT2sSRxFBy29~MSBTax`HIlbn_*Ds`^%c{Y+KJtt(>f4r#Ba|l9o-WQoR2yuR7&s`U_59yRq0HyY|+2^ z?Lo|XG)e!ekhd|q3>-x}Jeq{W+1JR#_V@lt)17P?A|{%&aB5!jQ+`nJv!*(h03a&K z-TTtwq%FLyRRt|d3R#$(u~QQpwCPo3rFTcKp=%6@j~M>x#VQ?)%M3>o1=O#5nQ)KZ z%%s60N^g5)<^Ye1ZU6gz&n_sH>OBVJ#ANrpLhPy&4+eX^OK+yt7dyS#IZD4G&%Kbz zr=#}(&uCsVcEf$&1uMdI=oXG)NUpJpQCx9G?Wk+=J{yq<-=a5lE?c4t_o3CVj~~{A zM~gilUvttzL4SIlJK$6$PV+pRbzqBkX)U?xOK?%j25j1Wt)(oPi2-HYs1H@mYI=F-AFl z!UA_R#E1OWMhU756{7~fc*d5q+h>$AX4zcOpq?@!-adJI^VSLe5ff)zp@!S*L&keO zqOFFKG+g~nNUcO=c24+6d`Cy%*^I8}(9+(?)(UVCxt9%_G_$7;#ZouZi>Inb9jWdt z2eu}C^xbbg^4Cm$fKV(!P@XV28J$!^jYI-;D7&de3-nyW_q@`yWCEO}0|pI2Tln>A3=r;%XuMP^G!q=T#K$M8xe(&vuQYtw@a%im%opRswN zlP|Y$gAy1e4XZMWx56y%z#*zDV6nl|uiM9=4$R~loMD1GW{g(5J8XFrzz@W&?B@K7 zB5i0nk{E?AE#U)Y)#2lwWxqGlc>{P;NEsn;68OyX0fcy?!vN_C^>a2V?c6K|Dbgq? z5W*Ws>=H+~R0i7H02*0KAV)p!cL<-suRepbMT0b&QxNZv0FW6IW#%wo+AZMLefuRW zN(VDZ-zRlmh6*BSZVt+A@?q%|cHi4^G7v0zW7WbrgTx38L zB%xmzFmBh`sauRf`&aJo7&GA@q0XKw>OeruC9Ss~c(JHx6%O$ z=GtraDBVYX{$Q2t7VJ5HRb=c2&>9+GR>!2!+Vb;r!?aNuiagQMxi-Kwn` zn65ftixqY|TXjjN_-95B+j~QyZqp;HUNX7 zkOXt^QFH0e`RT=r&|`}tXD4Nn!(cD5GhR8E>g66YcHXlm_knG67CF^_k1h|*U4~rL z#D(ES%vf@^VDy^%Tcb*F0CzYo>@Pl=syj47R_VMhRr!hz#H23!x#j8i%k;D+DVla` zeCt5YqAgn5$=<|!F(j(R?AbF2f`2lQ`dDb~_Ez~f7Uf%3I3lV;%)~ujyH13Y|2rVCgn~j=SNYcda8@+49yM5E7it;EITty_N4MiZ`|o z(=_m0b;Ip|<1jyGPBRCoYs|l|_P%erOyyG$v4abk)e~#WE^nNh7(@3FuPePa^xVdO z1Le(qYFFSAcd@K@^E{(QpESLMTq#@f7%;EP zfzrp$6Y;%xvQiOj>bx|_rUITvbyjlv#4Lq6EYlq!@{AalBFEz0xK*=JH0e?^c!4$%!J3Rpk=qhFR)(^-$0?K>%7u#hrQbZDcha3m9gJo;OLQUNb%TB6 z`0#r}jUDqYexW*k}UCA9y z)o=EyacZ90KAUz^X&6`sYw9s7*ruDMAYA;;I+x#(0jXI84IQ;G-TwGg%1@TD?2S%0 zWwj~gesWK$4IYxV?iqNtA;i=!B;mLXM~qqsro13<5`Xg=bi}hV``vussPS>VOrOsU zr+0PM;Oq7r)mgl61Og5`9^?@rtogoE${%px3KztBu<|5)->+5*07G}^eVo$n?}(dA z_xmAEBam91`d#+D`O;ty?HgWAoWQ|F?2vRs(heVmJ07G=x`9!S4Lq zqZ;Ub{LOm_Im&Z(qXN;G%$5~C$7^}=w)T!`MqH|lVUnu;5L{ibgj(%t{^KOT>`eA| zgeGH3gGF`dG|y#@d^|#w?g|vHL*o&RPJ^VULGu~$RXHPnNVH4H_m~05gvDI-Lb5OT=pF*cs**(vjh~k=00xT{EjfD zCfFLW*9$^vZDn_mz#|eI)7KeM+f71!{5ancJVCIMdo~ zRB|@{MKe@B&oi9F!xXCB8C@Ysd1C3sU4F9C8g>2C-@zNNi*kMcLQ_`H9ru&HPJoZcc_KTGw_8!9mJPk+EN`fb<3Mj z1o!$fhCsUmI6*{9_jaj|4d{Az8GEvsptr@8@0ZpDq2I+W zjsmwTB4l5l)(cj4`_kXPZ%%!`6IqqZq2&gz`I^Oe^QjsPh5H`cn;VT`!IVs&MxI0z zAMSoap4Tlit&LBAh+aNVE0;S2#N}HK+uIrQFOneY3P$Tp5@7fDiz1}H^$$Pm)YWHz z2qj4pm?WU_!oFt_@!8Ll(b_oOkbZuRfs>Ar*TIyGo=1|%cf9^3MA%;kRwJ=W{h+4r zBU5v@?FMa^Uxu;nA&9~qdCGO->Opc4$Q1Z_R_xsdvv0@mm2xF*tn`QK-;7;l=L zT$2Kd3`6W!j{R(5@fZ7!^}-;dkZ<_D3z(qyG+f4bRF(`sKrm(}06Bcj1#fy`B^#mq zH4yP>C2Dn7sYs9+v(NQ-*62haAq9$J7yeb zp5f^m&#da&S@P=Gscs!bN=Olw5dpv1vbDb795C4;L1qK4Rnx%1K%!&^fKZB0aIunN z2N97UjRq;<^Mu)|``$=~n>gyH6)B|PbHUY1gm3YQk->uOcX>2vZTsji7en!^Kocnc{651ndk zj^lH-vqWBkgav|t(^v5t%19DyT0qDen4LW|g1@eXAvY+B0lFfW??p0{hUP5U;;X2~G%L!Vi&b*zGyTa8d=L8j4|G6tHy0?BjVX`9Y!2o9HlsO=15 zQXojWqM3l^(Juy*IU`z8b5pwgkgv8%H)~fWA*kIUbm34~MZ+E^9)SU>#|nU`Zcv6q zKGqxBgkhNY^LC2{r}84JAW+CCi09Rl(K2U}AXgaY3|_Q%ywlmmKw`j&RI<6f*!S@) zYF&mVT~ct$Rgj5dEWvXl{DH{rqVh)4WT?(9LX?J6Mc;jx)7QcT&CD2}8iJnzrUrEd@QAb>OTX?T!i=E6OXha{zF z_-Oi_$e`oAKto}e2rYSEGfg|YmY z<%(m$=Djz3#k|~0|FnZ-TDSMz#VX6@xD~l%c~6(2>xfCXI`FXRaGozm@vy3j#|=BpRdcYb`dazh=tlDus+06&N22HNH9Cr-h8|}bdE_f+#7Fz0+O_i^cz^Fb4Un$6Chdi%fStW1 zv0+!@0Eb0hl9sH{6stnl4Uhq7~;12`vp3eNQc$q@5r@$p;O>XVSPnf%I9Hhz*~W zkT^$i7`iI}W_{=FEeL|!aq)&tUQYrTy(N3fYjwwHcFV;rk_SN^5f7x-JK8Ku7WXf! z+c-WAPa_kdw0o)b=i=fN5KgbKtap$g2omO=6jel{%)6?zTX zMUZAYLDQmA@g@k+B&Y=-xu5il8$=cc=+IrFrYy_FOM=zHumaVDjPJRZp)|@;$66a| z$&5a144hA&x#w2ZV5|q?>mI&nF?lI)$d}j!?W;-B2XC4_LvF>cWK{hyzlA#TuvRD?7n3iFge%(q~=_)&yB7rWxJOd|Y4rpXtvJ#ITO-SCGBf*AP1 zuK*;@>a$&57)pT`I@TpWp4=V6^v6H|GzbPIf2RHcE@3$U000co`UV&r&*L^KgK=SP z+)LOK!{>~-L67}ut(jTvSHw`-lT#h5J>xS=4W{gxTop@2+UdNAPH=2t511xBZX=(_ zpE9*dD0&=+uP#9e!DlH$+!YYRmbEswzk1TPtsU7R!Yt->$B@#!6064h;PX-MofdMPA2UGbfP+7rj2oC_-1+ow*^S*l_xsA z^9;`lx2IHKWpg}j=xKY?gXg`Ms&_nzAd-1|=Kw!2*Hh0>q{fKs9d!R59{w<&vbmIuRTw}j%&83m51L7>bY9Sys8XYl3h zaA;C7N$Wx*@XI(2-I3aGx1!PHJmL97$z(OKmL8>p$sYzP6a11M!2w4vfwE);NAGS* zS29j4wWi7ktW9N3c0?ljv!;dgh#he~h#T(vWSwTo3lM24S)w{$1hocbv!2g+*o;Vq zPimZ*I+!&jeYJCJem&C+-D_|$P;no&wS5MsDY&2^n~oPx0*HUYhXlZ~S9amQ1>IDt*gy}pr;645i_RfCLxBC7{^!El++ z+&C}r+|E5fo^=KwyA(KU$2>l^$z8_;r-Vx#<{3?za@P>&xVIv=J-ifkADfvF%jj&! zUU_a-pV>w|4O+^a{NYBuak>@9TAGK6r$NojuqjhV?p{I7oE-Q@Jj1;MuLQs;bRR@P zav)tYW8OJystP!fUN`ifPf|TN;~+!@*$7N32RdbH#JjU)HQ56~3IPu+PLhW|_CW%~ z1FE)oxj9vkE${HNMbTeg%Of)?kj6u$i7A?K&U%;N_hj7`i$belb=D(rA4v)c+L?OO z1UEEb*sJY{@g4Qv?*^i${-r$P`%?`)+aGq>zrg-e*c8N*q?~6#b!2ExG)5A;bt;P_ zX{^-Z%j?EwK1Vm22=x6XMCn0ciPG;p6x9v@9_0u@njda1 zf2_oknH~=xup`nD;9k*1?7I!Z;8I3&IHJiotZfzt^jklZ`;6V*K@jzP3j#VXIcq_J z52C(?UE8D2ttje3Pkd4~=h&KVUP*%&FUBz|fNXjV;N+mlMaIy#m$)P+%_UtA%X_Tg zZ~?95LGpL{h6bsX-9#=j8-z}e`{5pwRfX3?$Uj8bS_}dp~;@tAb4K)&@H$s@h{BlWl_c$A@LAh zOK{rrA!V`$rHjUN57B!vmEB^8J&5*H3->|)O57zLCV4=Gv|_JQVkmn zLBxK*vOFismd(4S#OZnJ$I=#kQg>u~WkwTq35B{jwz3P@lAREh4zHV`xX}~Nm$4@` zlZ|t(N%w9H8*T0}A>aJ7k{S;bD4T01GNN>`q7VU^GcVods#wthF&)W zXpe3%FfEAOxl&@`8$b710IZb4BBtqf4ge+t3Xq5&*n)c0`$GhLs%%K>>LMcX0A54^ zUQnuG@TIv!@M%P<90Wpo3U99{PWXeECW5wi=IWa2ijdFZxGPJR-d+>IgOevd*Z1b_ zmlU4S_uKZDh@L&VUgOY=Zvg!;LK&3QCKwcd1a40tlHu(&bm@6giK@V0>H;}xV4knZ ztVJ;rh(oy(q=AI`sd>yVD45KBa;I&*5gmQu#6YHH!yI0Owg$TB$n8cMt=D6<`hs^@ zpe;e9Bt}rIBk&Xo1c$u8K;aybAe}sYNKbDj9qL5EN`2PgCo=U~iHSzz_FZSYSgWTB zb-9}6Tmb>&0&XaTT$RmH`>?=}x*pIS$j5!bHJSooUY=NqaKV^TW}OLZ7hp;83R$>c zMs9XACvHN>3IZ(qaxQWf#MBfTUm2H(vnBijqIE&k+u$i+`X8+vw4IJ(f zBt*rtkY-yJgNOq381zRaOmAd32^DS!%wkz(udzxdEr(aAVVACG*So^xWs33HF1y20 z3Adc?yJ{=vB;D_$v4wZ2zdq?Axn36xSLO|ZP&BluS(@x;XKVtSM#ExD!)efcv{wVU z1Xj6w26`%~`+8XgoT70DEW7Q}-dl*i-#3IUZ>*h)VMjvyVO{*dQdt>^!~Je4BhnK0 z+a*MeWsiuQKu?kZ-)l;-d!oea9_w`^cQyjm_i-xrtLi;`IZ2bHtm-{1!a>1Z8SPWR zcu7t;9}!Z|Ij!a>3daT#8B7QZ_(a6RJ*knD>l2cK7VgJ--2&Cw2tdw#u!iKqqjo zXJDFlb~@Pn>E5GDM37l(E`}MhV~8~#xHtvAFc-qhO!_I0l(F4nS2Kt84n0n^6Bdr| zS26@Yn?CMvntAtnsLdvUds%~l zF?ps{Cota}8q}8O<62ji+xOA_0CmEUG<1^hq&Ot>Jx;OP+j|tW3%P~8pVt3p9}I5! zP)Sgbkua3hEYCnBCYu~KT4OQMaRryMNU}_n(=5AgX@INnJ^-GEsu}J3KJhAHUg&}I z+B9gfo7~^GIDqj-mmMJpwcs$kfJc`V0nJmAGFA^14<@-l*5Wv+~@8qcY}$%Nix#w9go#y`)O-|r79pA>U^0S$I` zKQuiM<7&vr^>pRsz$FhWA!4_xum&61m!8=3f!nkP!txX!I)bE`e@@(rMJd4qr+gvx z#7IrDyMx(i%k1pnI*yrHeE!PI0acO?=Ei^)XK9{iMtDV1m+eeU7OvRiRR&&`nH!e5 zB9U7bfWUQqQgcRGvQ1hG7_Y~VISl2;zL)_)*As$Mfs7+2k-egm*DNqN>jT|1qy{!k zB+AP~&xLzXiJ*5VeNqXCFwo9z5Q)FnllME7+=ztT`}!b(%ErlS#cxDePTYZApU|W| zf-7!&J8k-l6QLms1dbU~a@0R|pwwVwdzrf1VU)yI0qAES*ayOJSCHxjB6ab&Jt`*S zM^t@^6R$jS_@JdR56nw@^&SBf@;FbOZ&`}2Y7N;{m%(f}BC zxPK7IX@4U_C@ld%4I-P9thHhi#E%>3ZSVIlfYTFxgz|QE-Z6c07*}v1QTZ5}Z|; z_mrWKt`C$hLhs4gO8)b>C^f?%HVqTj=@J7!oUc`O&CuB$#46HyII70}(QQ=cgLl9l zh#7)11U|a-*>*8FB?8R4RLQ=Mhk0P{cT&nv+lyUY>NHMZYRy*>cG9kz80UVos_#vv zyAoZkGr2M{Cpfxif^$Lw2Rc}>-It*+ilTkLQFQW%;yFzc_>RZ=`Ip*T&pW5zU6q%` z{5(9C5s<;^t9V?(a1{FE$}EP8!LsC5;{qCyAxmQ*=+B$}5Y+rWLStEGVyHd>ovHlI zY|e920}O9RX}4nGTme?u*L&4C4G2;Xuz?_mriqp1#Aw;Z3=0%Izaz0nL`)IkELsEC zi_VER#RHtn9TB&^yJsS7l<)gTg!BRnPlo+@aA&c#7$|J{g6nuHw_5*lZH{8Q zjm>>{ObFr{4gcEXR6BtSYmS9CqCI1^v5p3oM$!cH_CQmdeL$=tX_JPgW- z3&G3N{IW77a=d7R1ZGP;T>C2s8JOG2K^C`e%dp09;_O_t{e@FRa-*^K%mXe&T&4Yt zgdH-1U81~8V>9JT;8;Qbv6~4XyAx802$?J|)xVeC3;@6l!Z6ITDoU_`hhK_DeU1KU2v`{kj|3s898h=H;^7A{)Blm^e*6w&NUIHUn{r0P6n<-Sq^>4UzjYYy3{IerT*ThlmwvDVT3_5bQkxw6ZvU^LAscjDERTb&TZ6a z;ogKHy};&6+x>$~)S?p>Rl&Vv&ADk;c{(839NAgD^Z{L2doedQJh zGkI)wlI?w6XH=8R)(#zm^eP=e%0Zg+X2b{xNEfAZkP<0MCjkPXS5Yh!5s0XOf`AAp zH35~X7ehbzPHjF3ZamT~_$8a|^&A-U4b; z!||z#>$W*?p6e&meI-2i6*vpkby?LAMv>NU6erdj|dE*oE>Z-8qigpo^RI+~+h> z0Rn0lq0qaNUmki|-2%UH!}2iPRLbp0x@w{NcBTH9gMOjLn!AzYW(mXv!Ze zF|{&KUofrY7B7_@dw)D~Z?q!*$$s$sn-6xC!pHNQ+pjdyZv$_#QPUSbEZ>T4*>o8) zPLu!m1ixR~ji{Ws_0Y-Wee3*`Xz-3Li;D&F)-#L&zf>RkG(^MBSKzd6(oP!tOy-LUTqz^`=D+%n4^2*fv<-RudWyUt3 z!PnyzS&XaJz-Rrr8Wlc5VbTY7y&j7C@@3kDxo(XnP?1VhV4Y?n|G?K(PtN1YUlX`g zfgWQnkdtepob)8s>i2` z;K*In)T`SYs{K*jxO}FAOZk=)6WZ$8^DIXxhsV;>!{GMB{!&<~FTPORpa-b5Johf} zQwzKA9uG{z%W|dV)Z^TP)dF#gT>#9RUS@d`>VDt=aT}UCzbpee9dDDN6(|>1)Fj@N z`vtWwEz#iLK0#`_t(w`@J`+3`PR!AwiG z@>qm@#w{M;&+_F!M;)WX3jM5`i>5f4xj%?p@akES#YJ^NMeoivHWoutR_Q;^C$_p@ z4%Ov@kG+V6Z?SX_?>?4zfPboy43yZNiw+5~;t>ke-LmPEYp5UTSdW=!z8!noe2u-Q zgvR!PbMgyU%M6+Kir3cfMF$REyKM{3Y&?d)v%JVy4xS%U`4OXr_JPr0?-L;*HXD-a2eu&4SUMdoV z(BX_8i*f=UB2@>qia}{{K=*N=QiU0xz$qU+IdG0&80)EPcC%SitpU451CeSf@j~cKp1CbQA@}v}d_PML5 zHzIga!H320(Oh3jZ*TU9UoU~56+hrY-f*T1=guZxO(f%68_^4}xsk&N2DcVJ;n^Vc z(o}jYoLK&SJO6WY&MrZgulOLKaC2s(gGkx6mpuukwZiJ3A&^kS)m9p7Kx}$w~itFfMfrV1yO9C@NEH`VsLSaM3Eq@gNjy>>q>W%yQh;nQ18bkb#8;#$W zY_$}3#YA|~-ksExe6WbS6LQFq;MICiRkg6$t(u7q*o)K{*YY`$ohI@O#6Kf)*TdCT zMye

drL3{fru7-ClR57`0_bOfu3sA+we{uagH}*#J$aMs?kVW#)+mU4})8(mgEJ z#7|tQ@RtZ8$4LeyJdB@H*h4X-eSxc6AX@)8 zQ*9TbKxseX?AKcYYtd&%oldOj<8vQvp|FkBLhF6pCo#@H_;)1v(h0^b?Vzp{)rMF( zlCH)q_TZBxp`uq?<>g^;g zPT^CAXR=3pZnptFiqKsNSS6)3fB zT2Qtl)(1jSJT7iQ#+~sxrYRdBXXe|{4Q#@c;fruzDUiAiQgD$ylYaZfhAoz(+bwFI z-wQz=V8alo|)W&vE0J${dl+>!=Wt0VT?_`QVEAx2?x#+xKqNj z^pt1m(4-yxfEFxP-|`6kNMPDeWgVijzRL}cQJ4=gN=CbzmZJE!v}NAol;wupGWH9y zzX#;ye6MB!-9lVRK!DZVurYbWm&wFrUz8$6ndHDqVjY2-1G@2!3Gl9nEp>WvB%XCs z1mA}hj)fO#c0&s^-4Xzs5tHJGfJvg+J2!Kgy^l)7J3z7^Uq8l{g%l;QyF!m(~Pw9N!0eN|W;u=A1pi70eLl_Jx zt_&niX8Ve3mg})7vd1oK%!7&@V1|G z8wlYUPie(mBA=q!7j55Dx1K_(N|JZa%)I3$s)tS?K_>4glg1`Ei*>AXJi;}v9RYQp zf5c1{bV%%M(nO;QI55Pko^bfMUvp%1x)G%%IcGkxFHtu zoOK=aFv8m8o^JAZ!fm$x8zOoS+2&gs43?yh7bQ-9z1# zJOYBftbzjqy@Er1yh0RVH~h2j8d&~(WNnR@UJ8{>YJAA}#h~`SwDdK#$^6Ys^})e9 zyElk{6uPq2>6!@SS`T(jVKi!_nEjQ-PSXc96BSy81EZxkA1n#dsLt+)Dxpb&peY=X zGSHL`bkAE*(EBnOOW+a)O0VQ8pPGya5RkA~J{Ocre?BE}+^HiPHkKoxI0<8sG8;N& zJr(g@U&h$o-&^vmoRErqU-OEQfh2S2<8x8Q*U$H8Ry+de8TwjPZ~CRCj6%Ly`_ zIbFS&UcwSyAF0q?y)S0L`^Q)>;v#>$8Rpr2@yjOUPzOWg?`%SEKD+vv%Az@yQIY>; z^9J<)^Xbyx?)zT(IO9}Q(YrmA7AsF@p!1l4d$F&??nnj23k z;ESs{YNgYEWBY{V!%R7-FUOBM}3Gt*g*dH*uOMKK*IqN&8SKloHD zIOgIkOFn2yye{eo-_(Dw$T_I+RtLsufSXExvZm2$ztiX|a9_vD;Ex$$YlB;#ePsz} zf@HQXjP$gZq%)Sls$QmgyuR4khn%vGUhWzqw@qS8ItInotC0f|o|o~cA8+~Pa(rwP zM(8eYvHVWN?uv>}IhBA(YLWgoA`D@nUN=L00&f1?VJ^2Dxe~;A08!}pV&nBxW5#;Z zC7vE84!G#kSUQ0|9Ddawxz!^FEbek zYIh$Vrn&k^jZbm(^q(YVwpQ?%9c(>D+h%DNVA^1H4CLQwq4>aPz_8n7`Pyb^~Hcfo>j0O67+akqUqy_N1wL%x# z2ivGtYEYM5_TQ|v@$&a_5Aia&=^gOX-mu#}Z1Kt*$!mHY-L0oP;-yO_T263J2TR$P zbBxwqhr^vXr#r?!?JB9NMbxwEC0(||b-Y=FX{6rXH5~coaTq#LoLQ)xQNh<>B|w2^?^fy4ay^mm>nbLR@|l za1@|%TW3m^3Lr`k0I>X~?M>}<3-hFF{ z$uqzs>;`oL|1`3YsrU^|Mq0qDOsr?L;2MJi60=Veb7K=8=7GWyZkxQG?F!0q8gg) zhDUbU%N~YgdB=I)^Y-vOZ}&O({l|UI?|aVmxxT;O^}XOQDj+ie005C+KEPm$YN0Fz z0Dz+c0M3yASZjK?`y$SI&&lwr;y&olXMo#89Dw$X6a899}knF(7R>*K&w6vc?W^-QFargtVOzbXz@nK~h| ztWmbtadigpq5@Ah8=z&JHEOXl&$6!^IUvQITvm{H)r$3dMHfK~pz>cKzA9B6az_ z@%K5$%pP8?kTRJ3<<6*iRd@mEUMMKE!dtSg@`CDa0hC4Z_gZMuKDd@t=_ci&e+Em1UwNM`dZzW`+PcrVTML>4 z3Gy=4#!^^a6)fta)jUhYObPKrz6jtmR2DFDs~n_Fj*)}$6YuR(c9MBC6ngh@}%LDW}qGlSLc z3A}y<{^m0=bhU6Tr~0kcO7h@fT1jl^?2i`1{6uZbo%Y;Ci>M6^jZMR?gMw8kC!^&l zV0Sox=EblNa+R?CP_7iJY&Gk8N_WRVKp++*CInJFckUB${7nm55#@g+PIycoz7Ygs zh1Hr5=#0haF--8Mm+;64ohBK)uRdQLyNNCpa~`OCI$j=pF| zCy5tPNndk<~fk?iI{gn(Pew@lM?h$d082H}z#M3=kjI=rH|`t+7*!5SIi>fH4U zE)TbtGl(?B=t-zXPEb5-IiT@C?bWl#U8%)j+3b#(*alC?25|mEUTvq-%KG9G;Yq;d zv+~BM1if&<=FOoi28&{tPr@K{IpD%lQ&RSmQ4X_&ZPtox$+Q$_rm~dbqVw$Z53%71 zwIrLiT3rkAlqHvFx7T^*6gg2Ss}J1G3H41gZv}^KC~DlqKgYNj$^5t{;xPTa&%|b6 z_xcx2cNbCPo5F4Q>v|R_TLRk((arI7&q$A&=Z5=j5j}*@WI(WUBT865f%xr@kcI?j zX7B{xlO17cdf&jto7sBmx-1c8**NGISIzSl`Ot)VQ{zWL+BsG)fRX$5>2n?c6=9Vy z-_4y4{Nki&vVis_@QqK_-0S&w7EN-YP771}ZFU}}6A^0B>ex7K&NQyj1!R81mr<{Z z#T3X`x%orYK|}x-sZX~$X@JyF+9k)&wcxKhiWn{JvK>&AXgAz%pGj_CrfqPT_--5T z`I^G^KExM?!B)_XJXP2U3swEG)MusR935P-DKTpi-j||UJFkpDM{)Ht`4pXTaJo2C zqm@xaFU>_wSlXrkPsGN6n!)gBJo)0s!08001Mo z+x}|~4?kZ|zrU!={9L>eH`I=tnHXIB}h!yMXpz8Of`TV34ZuX*X&eB+O^lE#)f{*)2v zfeO}(cdvE_=7X{d6)Kr4St>6Xqw~e`#=tmG_-grfwA1e1FmvFergVuacB(j!&+IcW zTe@;*{6rw5_iW2cmMa4oJ{e{_hMU7w?%8SGu82?wOkjvTN=vLJ?J7fR>Y_hBXD0et zsin8wD`nDdjhcLL8LY2>dO}8l$i0^sl=Ve)w)~dTOX8P4h)=_M&mzT%V0NF&cD+;dVqvr-2a#7_guv${&W=DFd8W$CuhmF;XJeq(%12Zqpza`!owR0_xA8adi%N{eGcbJay-=N zJumx%l|7bw!7vv;<6eK74#V^`%iOgq6V_KRB>{$(#@oJK!!A$8*LQ7APX(9ET-%zb z%CLwmlI-w)+=PLO68pD{Z1XMHe>BBR?>S_*D6g`MJ1gJ-=*n{cAVcZIXbf(IjYfusVi^`SKET9Q^ z9vVrT+5I@d<622sg{S`{duBI92IJ zk=KlCQD0$#LLi!fR5dIaB<|rp%;FwhZ<(iP@Tfe0;ISPf{q42sf!)*CSQcErT3%|B z@$K`fC!La~jZzK1(;lT6;GSpENv3E(Cd&19nr;FA zhx?(!Y-1Z3B6-;X0a+1c{7!wJsr^uM3`_C); z;qf<^Oev?+k<8-Fz)GAA{gj z9H63b1$3topLyr%i9TS!bdcc5{e>(NhU5bsz6Y)?&2@K;5iT~7Zn|DZ86B~CUJYW_ zO_g%e_nPj$_8G!K+|cxiqif~VYVK_!uThS?>B7HTs|gZ~bo4>$y5ICTT&-U$fgu6{ zez2l~|JZN|A0e1OQh)Z$q_==&G5F6)=a3L`WAu3W@ge%FoQ53Jelt>!3mu;y zzl6-mx0e42{o6D-E_J*Ee@WTV{zmFZKOWaS?(|=pdMAIQdCd24*a>nzIlPB~01D*& KfgfHB0RI8dzp_FA literal 0 HcmV?d00001 From e84ef24b3065cb2c4a3c60952b4ba9ecde27ab42 Mon Sep 17 00:00:00 2001 From: Abhay Mathur <66543532+abhayma1000@users.noreply.github.com> Date: Wed, 4 Feb 2026 11:22:26 -0500 Subject: [PATCH 13/14] Deterministic errors scripts --- deterministicErrors/accel.mlx | Bin 0 -> 3782 bytes deterministicErrors/gyro.mlx | Bin 0 -> 2953 bytes deterministicErrors/mag.mlx | Bin 0 -> 3586 bytes 3 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 deterministicErrors/accel.mlx create mode 100644 deterministicErrors/gyro.mlx create mode 100644 deterministicErrors/mag.mlx diff --git a/deterministicErrors/accel.mlx b/deterministicErrors/accel.mlx new file mode 100644 index 0000000000000000000000000000000000000000..9987344e28d6d71896d42c78e92a6a57cc3bc368 GIT binary patch literal 3782 zcmaKvcTiLN62~JYfFL(?5Cl=Ah>;$UrlCs>2sc0gL5j4{5-C!oNe_sCNbkLaNN)nc zh@fnCXrQm+9KgiekRib za03-GM6f$6s;OJB*AIeYm5bA_{)jdEQ{~vSq*&PbV&P!k7u@h;U__i%f&8*mIm}$U zC|OfZ%U&oallMLoZ!N-1$w_0HE-y1vYkhX~d_-Ld2HfG6KrV64en+mR)L(vqQfN)K zkj`t~AVo(*S#E<3%yZaGh`M?U5)I6ulvqj?c_bkBYIp#Df5S~S>8ys^u1 zMWjo2i*OVrxGUu(&3_B&MOGpmilutq<5X6EJ2Bt+z*js-tvmZFQ{U^2A1JQ-UzI1G zN=v@f7caUkUqo>Qo*QqZdlwQ{htMo17s@tbL_Ji|!Zk$=%-{yUTM{X%8ZR_ry6s|)#8I0*D z-Uk7w%BN7a>$sockV>eW$)Xe8?R_0SzF2Co0JQ=a*PD^KW~}oKM^Cjl!C7tCmJc8A9dQ^3b{&%3CBvxYzi9oA}K!3F@(hBWt zbx*{`)%pRF*uB5nw%1T!Z$XqUV6JN2NXfaVHc?DwdHFp$f@z+TX3ZfLmKs)3tW(;9 zNITxwYK{$p&^*zdzf$u3UDo%ic#*17`4ASkY|^F8*n=P{Gtrd%d)z*ccwA!#+sZa< zIryJ~QP)A&ES9TdS}E@_Fy>AMFB$IIc0I-@XeHfwQJ9)wP?`^W>Rtgh&y0tH(&N($ zU!f9fv}0PL7aQRN+kyNGurT3NRqn#)?s1Ksv{Nz?$1#KI773~3eN0$yV2qtZx!^}1 zE(*?U)9#zaSm8r-jfe+AbuS>3XJl?7p%3@sp(!gUXkN00tn|eyw=6tsWz5QTv?P6=kxo2CNc}K}zf_3qn7+TWcal@rgNxd1sj1W(LuLliXPojIm z7uNHoQwJ1T4Nzgbs~FD{pB=V#K02ofo>iPif_Tb+|9e_`{Ov!-! z-@!ov_Rgd6*Y4KpD3xi9fMMCvl9MxR)*C)#(o$IS~|J+0Ln_ zr=VWvaXjxZw|xqJZ=5q$%|Mv}R8CqpEMBSJ=<5!Tmc}%Q^*P5lM6FCr51{T_XXb?L z;sY(fw-)h&hqFQ5GUQ1ipFiEXnOr^IB+OMc&0JqYK z@dx5LWoOPh8BcLf9JbgzvmBMU##L!{2;VzS^{3i?ZIB+J0z;>9ba2;Nq}>4HEaAlZ zO?@%q5s_^R+}vBfv!OnOqksf^1CI&E;C`^aWmSI58Hj`o2SnVB5j*ka@{{bsAXxrc z%3)5msT=&d*F*(3J0@@!G;V}_uJKXC9SPLT++?_V^!(E6DL&|4QHQDTtlg5=M4qdf z-Z`6ZAy4epvs`%;XWBXa37*X%8?PO}eGjE-B}AXcE`B^KqOa6!-EqAiV*%e*J6NUU zF#4ui0jzTYuh*4Hd*M-N3rTZwOPoY@qU>(xDckrTwE^B` zz28!m;9Zj@CAv8QDU!kz9l8NVSS8CLd|+qkjEG3C?4W)1?jJY;>MxhwhpdT>gxMLQ=Eqow88ok?ARu12OpMM#v@;G%oV3=cf_)?3-!H zpZ7}V*HS`#)qA-4IkQh9HPKJrj9FZTtLsnB6AlkprO$}qUiv;kj!tS#w4AuYC*-sX zJF>J-ditX+@SAVmZ3>_d!dEQTihO-8&8+F8yG;)^js+g^9H069Z1Us*SJ{apmxBxd z;QVzqZP8XXR%j~`YgZ3jn1`#Itq0n{7WHfNV&b8CA)@SIgaZ~G0!%Qr5%>XGm%z_| zPjWVO6C*V|{KDwvBiC^9qBVStPZj|;K>&~Yj^+GOt-I6QYy?4 zKg^_Cze*+k1kmR_s22>i%57M;^d%qHaua<1!unOB_fnfxs7wucuQ?a>g;z*dmfcSd zh4;lX8J>4`Sw#&D_q zYXh6mGlx4NW8{WEXn*gl{e_e-r9=R;#E1W%K&W~XttraE)#X={na9d828%KSe0Ae* z^Du%a(*u*nSswA`KOlBeWo$VC{`Qe>U+7Dt%=(`t;F|%H~NP z#D}EpoGM+zoy065#Ig(iQ?>@S&bC%4+uJU7uD|kq5R0P^76pY7)|?_pY$z+aKn#9M|~A8(>J1OWa8GMq>n literal 0 HcmV?d00001 diff --git a/deterministicErrors/gyro.mlx b/deterministicErrors/gyro.mlx new file mode 100644 index 0000000000000000000000000000000000000000..5cfb87ca57db300e78222afabd2c5275f9dd023c GIT binary patch literal 2953 zcmaKu2{crFAIFDbj4X{^!GY!vbJEd#3*Y-uTXYP zN|q!`NJ5bzTX@QQz3+QonWv|F&b{~Cd(Q8B?)P)Q|KIr-azUE3jgWw|duhG=BcmtVwtTh)i# zj7eIZ(Wl2n`STL%+oU?eRVlohDW|JOI_)g=f7sqDks*i__Y{7l^hZPDQk;w6b1G$6 zdo=lmi6-j2bUsPcolCSDXQ%6h9Ahmckx)zHuLa_2?j^}S^-BjUv7KMnd~iPk{((_? zQKOhOXv!nQvyr*5w47!Bt@fc{Og$SKMIMb&nOOQBlBeyTlz(DbiBu(4%f|zo;}Ez+ zhExa0Zf5o0fr)wbo^YB1n}G*|rKVnUGnWQB_g)Ls+86kXyVE0k3U)7kD0x18%00OZ9lpO=`1ftxD-D`Pz z3qY4uhx$E1YD2|KO+v>zi2k11)lR0H?Ovs|#u;}B+hK~4hHZJrxVoM#uMiKr&+84P zsVUc;RwNt4$qYxW3Q{dh9aU3ma3*D7={yTgVyHff@;I*AozSu=j#C@qG)q+_*All# z3f*1{&7CHNtEuIRLh6^5c?Oz;u%f8h6&@u$i0lzv`s30DrWk2X6IG{UfhX5;cpYwG z8>!gGZwDflN|p=ip4w0|hlg*LCP&O{cA6HY8(43>C|t0LU)9xJGu_xJUP1_RShF(p zMuM2iMv3?(%C}h63WTQ3j2El%7Dik=87eCYJ$3l-^MQ%SorIH~7Y$RS#?jc-5GXII z-eL$jo@By4DR#S5SXuJmx*4jI{s)?g*kfJw+F+G^KhQt8%89Z7&>Z^z#@5?8kl^eh z=jQ8r!5c{LZfwiXV7%xRU`^CYB-kMOO_{7L{sNDO$0tn5sdyyX5JTxoOI+NdX1{a! zf~bmiP^7%*&fbVQlz6O=!XA$8nRz&bk*1T$F~68Qu<>bS4J^dQgqO&DD^P_zG9FX* z;uGY-Gkz8qa7=84OmpYR^N=94)oBp~-!wNzIVir@ijC5z$W< z*Nhg`vFb003Sr>_W?f1ZYf)A(q$%9*ko%m)pig-rxdPf?NxmaaSKnnNlb=x1pj=dxf;e2Xe;T_aOIpN z23^({EVha>I-UU<`7+V{*^<0CncDbjWBNnbwU3rTbKo4SCTRtfw4syRgw#ORw|fpp zSrWrx#T@p&dF`4@{IaDsqOv!dt}qSZ{1A~weQxj@b?w=(%o$sQ47)WxryKM2-9~TQY!=*lf-)?AzW~0X|ahiOK11@3+)Q< ze6aeLSFNs!skllBPyU#|n<;iL z!|u^nt~VsqmJ4_Ekc*cZU4ziW5P@&SU)1&bTdW_m{}l#*(io7%a-EJ3%B1BwLm&%$ z64ns^%S6=W7_U+(>|>-^QM;OGHK|w6=Tycgj(*uk8o?ZhiOGZls%n$KjM?l@;G~C5Ny7}vL8gHT}{h90KhnKO8gFlK`^kr5{%=vqH8vx+;Xi3Epf=dL$s%FJWTrXza}zR56eXFhtqx$ep*lh6i| z4I=*F^M{_*5Y(2wnX$dWnMx-ihn(ELXu4A&v59gX!mZ)cV|ppJo7Sbhm2Z-?Af=mS zPUb>ifkn2nW`@&Qsn4^}kF(3lFK4Lh)k%B6&0#~j8>QkmQB&}?=+>E7$fU?7Uy}Ha z_Wc6B{bmLATl$*;ld8a9m-?OAGk5~tnTR*`Ip@1;_d+s-JxU%HLxp!dYdYQ$B~dii zc!+B%K-~I1?9IdT;o&%rsgBVX8*)m@A+>DqXmg8+jyh_vYFzS$!QeaB?ZC18q+I!H z6|kt4>8Se+Yt=7}l;)Y&H?18)ZDf!SVktal`r_=Eb&!&LD%%beG|2|E%EB+fei1IY z1Z@#7SbULWWNJ^iU%c2TSPo0*_LNVl8o*8$g3#|AxBNYN*uxpL8O|06#qh`9h#LCb zr^snIDqz3hR19-aUcKOMgqrDz3c=YqZJa{$_(rUcqT+Wl{@ z@Db42ez6|-ZO#AKUc7;APy5fb5$N%ri75V8&C*s_!& z!>fe~W0(DmbIx^odEe9XT+j2zb6xlMx_+PEeSe?t52jBBWCZ{Kl*ET0@S#_wRF(t) zs3ZdbScqS2HN3t2;9h<=&4W>Jw7otDKE-a!0Zi5 zGWC@7-9(;Y_+44}>ul{Hp3sl91sII}Dt?0V-izl+;;&I@WHNMa+e)=%ValJ#MOGAx zX#?lo(>+?L3d$>Ke!N#bA7t1}XP{p)9Va`xdKjFi>XY5%~idf2*VLTi)$G*ESOJgWi`Xf5e~+7{{Lhj6+j z>Ei9|k0f^QN!wPMsvCDnv9=SeCLBPZZyj!fjt{k*=R5CeDl^gB?{mo5zN$BtJ8D}7 zuBqhkc!cyNIbC)5INWR^~n{ zIWOiGtqSMfiuVxgGHf!>&bz=6J1TBC%!zbC)&o3q?JVkiSZC@I*c0yH3j6!FI+J!odXwTxM#qaNS>k4M-kU(ZX9fx zL`($iP%ZdNSne=p_0F$(x|w&zTV> z64h}e=dqKAxI2e0^l3=hzvk2)bHE?dLmLC|qL^0;i@}NE(Sz#en+2Yin^~3oK|&L= z;qYNCv1WWHCk$|!wAE-|IS0`k*cku-CSvRUm$bLPAIksVoQJHC#?w-aN5j=M>pn4p z?vK<{J$vE=4X4!d4fdA=DH2f*E=mbYT`sAN@8wmwd2q8-bdt1)6kEZVm#+;mG=Jc0jsllZ**t z6_}*bMT7%kD&=MZ6K@+Eew1%oC||5y_3goCpZqN55bsHKq+y{_8#qf|*Y*l~xokJP zG1bCT!zR7VjhUJ5l~Q$z%YLS_vSME{;b5hI)dSB3K6ZpxAGaUIV|R`Z84{~yrz%F~ zlCzXUbOx0ay9mpa5&Ow7tIMrh7dLN@yuovkW8@ooYCPgltb6#;G($jQU4M>Q{&~U_ z=kmpzjsTZvY^_mzuI}8vZA}MqX0pr!Rj{%nB#={jzXKrS<4J3XpNK?nAaD@6huB>+NK^ zJc~xK0|#Lzp>)(0;qsqt@fHc@#PmJqvs=9HE6h|E4AfY(W z&56a3Y$@-9E(^=m?3wKKB-ydCdW*&g?|8D3mHFD>rv%(2LEJLr?K6fNqwSU+cgR&z z@jdPDjXfhTP~0<}}tqPX%DK5~nhn_6%=OX@N{*qejjXi3c{ z69|b%P1|C_f~GP!B=A8r0tH|pYGv=Hh0nqdh7dhM@JqLgf?v)Ey8)x1!@Y?XBOm#ag!S}Mk`XrnVm4b{Vkj!|W>QOhHZY=;-{aKT*QGrjea z0nt6+X`F#xg;v8vjD|#{Jb%WC4E!rl`!}5)#9u6EE(Fu{v;7(!YcI-fO5J#d<8lb# zEnCuaajxn5f$0(FOhNgzf@2NnN_eYtpuq&Y}cRQBYOIJ?A=T@6gjZldz?}@uLp>WD$0Z zceVQID8GyOLi>-G>>?TNW-)Kc%r|IGgHWZh%q$}Uz!M+A-+|ByBE~wjySLX#i`gd+ zFh}#Ts_lf6cVD>Qk_|2`zwxq^fv>e7nS|akPz(cVz2Js+66 zHnX^wjWMRMG+|B)$Zfw9FB^8Nun;K?C-hP%gH)ZNm$cXEo2|$Bi-P-t!DKgh(Bz|CHkCtMLBPe70 z+r+TZf<+%&&$Gc&4Yg&O$4(FKNXmeN8|ai{%`IlJjf5b%d&xUmZ)cqM z{XRZR%9Tp2VvOGS99{Nmt8Pd~<_FdGuC>D*n=8=Tc-%SD(R+4OYEU6|*?k9`0mcTf zNy8yXAD}F>O4%upzcQGlV`PsgD_&{ks$@(U@Q_NW8Har?02s{N-1BiCrVjRF+)4)ROrCGlkR zKYRW=KRGLPb_Rb*bx{3A>eNJ@)jZqjzcj0;f1`P(?_v7npp!~U;(d*{KlLYv55T_w Dm1xZh literal 0 HcmV?d00001 From 62373805fe64faef1d24cb40c14d45adc708f711 Mon Sep 17 00:00:00 2001 From: krizzyegg Date: Thu, 5 Feb 2026 18:31:59 -0500 Subject: [PATCH 14/14] Added allan variance script, including output, must change units --- AllanVarianceScript | 1 + 1 file changed, 1 insertion(+) create mode 160000 AllanVarianceScript diff --git a/AllanVarianceScript b/AllanVarianceScript new file mode 160000 index 0000000..668dc23 --- /dev/null +++ b/AllanVarianceScript @@ -0,0 +1 @@ +Subproject commit 668dc239ffb67fea1afd4c66ea71860e5c01eec2