diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..2089f94 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +.pio +.vscode/* diff --git a/AllanVarianceScript b/AllanVarianceScript new file mode 160000 index 0000000..668dc23 --- /dev/null +++ b/AllanVarianceScript @@ -0,0 +1 @@ +Subproject commit 668dc239ffb67fea1afd4c66ea71860e5c01eec2 diff --git a/allanVar/allanVar.mlx b/allanVar/allanVar.mlx new file mode 100644 index 0000000..bdee812 Binary files /dev/null and b/allanVar/allanVar.mlx differ diff --git a/allanVar/standardVariance.mlx b/allanVar/standardVariance.mlx new file mode 100644 index 0000000..a4b2fbc Binary files /dev/null and b/allanVar/standardVariance.mlx differ diff --git a/deterministicErrors/accel.mlx b/deterministicErrors/accel.mlx new file mode 100644 index 0000000..9987344 Binary files /dev/null and b/deterministicErrors/accel.mlx differ diff --git a/deterministicErrors/gyro.mlx b/deterministicErrors/gyro.mlx new file mode 100644 index 0000000..5cfb87c Binary files /dev/null and b/deterministicErrors/gyro.mlx differ diff --git a/deterministicErrors/mag.mlx b/deterministicErrors/mag.mlx new file mode 100644 index 0000000..2e2452e Binary files /dev/null and b/deterministicErrors/mag.mlx differ 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..7f36563 --- /dev/null +++ b/platformio.ini @@ -0,0 +1,32 @@ +; 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: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 +build_flags = -D POLARIS -D ENABLE_DEDICATED_SPI=0 +lib_deps = + tomstewart89/BasicLinearAlgebra @ ^5.1 + Wire + SPI + Servo \ No newline at end of file diff --git a/src/Conventions.txt b/src/Conventions.txt new file mode 100644 index 0000000..ecd6297 --- /dev/null +++ b/src/Conventions.txt @@ -0,0 +1,41 @@ +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: + gyro integrate + accel integrate + accel upate + mag update + gps update + 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 + + + + + +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 *, +, / (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 new file mode 100644 index 0000000..1cff4bf --- /dev/null +++ b/src/QuaternionUtils.cpp @@ -0,0 +1,493 @@ +#include "QuaternionUtils.h" +#include "BasicLinearAlgebra.h" +#include +#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, 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)); +} + + +// TODO redo +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; +} + +// 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 + 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; + +} + +// 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_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; +} + +// 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); + quat(2, 0) = 0.5 * vec(1, 0); + quat(3, 0) = 0.5 * vec(2, 0); + + return quat; +} + +// Works +BLA::Matrix<4, 1> QuaternionUtils::dcm2quat(const BLA::Matrix<3, 3> &dcm) { + 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; + + 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); + + // 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; + 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; + + +} + +// TODO test again +BLA::Matrix<3, 3> QuaternionUtils::dcm_ned2ecef(const BLA::Matrix<3, 1> &lla) { + BLA::Matrix<3, 3> R_ET = { + -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; +} + +// 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; + 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; +} + +// 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 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 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. See g_i notes + BLA::Matrix<3, 1> normal_ned = {0, 0, -9.8}; + 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), + v1(1, 0), v2(1, 0), v3(1, 0), + v1(2, 0), v2(2, 0), v3(2, 0)}; + + return ret; +} + + +// 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 = 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_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_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); + + BLA::Matrix<3, 3> R_BE = ~(M_r * ~M_b); + + return dcm2quat(R_BE); + +} + +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 + BLA::Matrix<4, 1> q_EB = {1, 0, 0, 0}; + return q_EB; +} + +// 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 + 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; + +} + +// 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 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-2f; + float e_sq = 6.69437999014e-3f; + + 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 = -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 degs) { + float pi = 3.141592653589; + return cos(degs * (pi / 180.0f)); +} + +float QuaternionUtils::sind(float degs) { + float pi = 3.141592653589; + 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 new file mode 100644 index 0000000..5c57e8b --- /dev/null +++ b/src/QuaternionUtils.h @@ -0,0 +1,208 @@ +#pragma once + +#include "BasicLinearAlgebra.h" +#include + +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); + + 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); + + // 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> rotVec2dQuat(const BLA::Matrix<3, 1> &vec); + + // Small angle rot vec to quat + BLA::Matrix<4, 1> smallAnglerotVec2dQuat(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<3, 1> &lla); + + 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; + 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 + 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++) { + x(inds(i, 0), 0) = vals(i, 0); + } + return x; + } + + BLA::Matrix<4, 1> normalizeQuaternion(BLA::Matrix<4, 1> quat); + + float cosd(float degs); + float sind(float degs); + + template + 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); + } + for (int i = 0; i < M; i++) { + ret(N + i, 0) = y(i, 0); + } + 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 new file mode 100644 index 0000000..84963b7 --- /dev/null +++ b/src/kfConsts.h @@ -0,0 +1,232 @@ +#pragma once + +#include "BasicLinearAlgebra.h" +#include +#include "QuaternionUtils.h" + +using namespace QuaternionUtils; + +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 { +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)] + + +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 { +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 { +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 + +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}; + +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 + + +namespace Max10S_const { + 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 + 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 + 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 new file mode 100644 index 0000000..d74968e --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,127 @@ +#include + +#include "BasicLinearAlgebra.h" + +#include "QuaternionUtils.h" + +using namespace QuaternionUtils; +// This line is a game changer + +void setup() { + + +} + +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:"); + +// 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"); + +// printMatHighDef(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 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 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}"); + +// 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}"); + +// printMatHighDef(quatMultiply(random_quat, random_quat2)); + + +// Serial.println("Test lla2ecef: School LLA: {42.274027, -71.811788, 10}. Expected ECEF: {1475354, -4490428, 4268181}"); + +// printMatHighDef(lla2ecef(school_lla)); + +// Serial.println("Test lla2ecef2: School LLA: {42.274027, -71.811788, 10}. Expected ECEF: {1475354, -4490428, 4268181}"); + +// printMatHighDef(lla2ecef2(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 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 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 extractSub. Expected {42.274027, 10}"); + // BLA::Matrix<2, 1> inds = {0, 22}; + // printMatHighDef(extractSub(school_lla, inds)); + + // Serial.println("Test extractDiag. Expected: {0.8662, 0.9699, 0.1834}"); + // printMatHighDef(extractDiag(random_mat)); + + Serial.println("Test g_i_ecef using school coords. Expected: TODO"); + printMatHighDef(g_i_ecef(dcm_ned2ecef(school_lla))); + + Serial.println("Test m_i_ecef using school coords. Expected: TODO"); + printMatHighDef(m_i_ecef(dcm_ned2ecef(school_lla))); + + 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)); + + 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 new file mode 100644 index 0000000..84109a5 --- /dev/null +++ b/src/qmekf.cpp @@ -0,0 +1,405 @@ +#include "qmekf.h" + +#include "BasicLinearAlgebra.h" + +#include + +#include "QuaternionUtils.h" + +#include + +using namespace QuaternionUtils; + +void StateEstimator::init(BLA::Matrix<3, 1> ECEF, float curr_time) { + if (ECEF(0) != 0) { + launch_ecef = ECEF; + } + + 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), + 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(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(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(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(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); + } + + launch_dcmned2ecef = dcm_ned2ecef(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 = 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 = 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); +} + +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), rotVec2dQuat(unbiased_gyro * dt)); + 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<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(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); + x = setSub(x, QMEKFInds::pos, p); + + accel_prev = unbiased_accel; + vel_prev = v; + pos_prev = p; + lastTimes(1) = curr_time; + return x; +} + + +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 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(extractSub(x, QMEKFInds::quat))); + + 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) = -1.0f * 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); + + 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; + + // Process noise + BLA::Matrix<19, 19> Q_d; + Q_d.Fill(0); + + // 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> 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; + + // BLA::Matrix<3, 3> mag_var_diag; + // BLA::Matrix<1, 1> baro_var_diag; + + // 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>(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<3, 1> a_b, float curr_time) { + BLA::Matrix<3, 1> unbiased_accel = a_b - extractSub(x, QMEKFInds::accelBias); + + BLA::Matrix<4,1> q = extractSub(x, QMEKFInds::quat); + + BLA::Matrix<3, 1> h_accel = quat2DCM(q) * normal_i_ecef(launch_dcmned2ecef); + + 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; + + BLA::Matrix<3, 3> R = toDiag(icm20948_const::accel_var); + + 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<4,1> q = extractSub(x, QMEKFInds::quat); + + BLA::Matrix<3, 1> h_mag = quat2DCM(q) * m_i_ecef(launch_dcmned2ecef); + + 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; + + BLA::Matrix<3, 3> R = toDiag(icm20948_const::mag_var); + + 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) { + // TODO something is wrong with gps vel + lastTimes(4) = curr_time; + if (velOrientation) { + 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 = vstack(extractSub(x, vstack(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(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 = 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, vstack(QMEKFInds::vel, QMEKFInds::pos)); + + BLA::Matrix<6, 6> R = toDiag(vstack(Max10S_const::gpsVel_var, Max10S_const::gpsPos_var)); + return ekfCalcErrorInject(combined_sens, H_gps, h_gps, R); + } + +} + +BLA::Matrix<20, 1> StateEstimator::runBaroUpdate(BLA::Matrix<1, 1> baro, float curr_time) { + + BLA::Matrix<3, 1> lla = ecef2lla(extractSub(x, QMEKFInds::pos)); + + 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(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; + + 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<1, 1> R = lps22_const::baro_var; + + lastTimes(5) = curr_time; + 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; + + 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, smallAnglerotVec2dQuat(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); + + 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::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() { + // TODO + return 0.0f; +} + +float getLastAttUpdate() { + // TODO + return 0.0f; +} + +float getLastPVProp() { + // TODO + return 0.0f; +} + +float getLastPVUpdate() { + // TODO + 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 new file mode 100644 index 0000000..c43497b --- /dev/null +++ b/src/qmekf.h @@ -0,0 +1,172 @@ +#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 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 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 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 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 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 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 int bb_z = 19; +BLA::Matrix<1, 1> baroBias = {bb_z}; + + + +}; // namespace QMEKFInds + + +/** + * @name QMEKFStateEstimator + * @author QMEKF team + * @brief Integrated 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; + + float curr_temp; + + /** + * @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, 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); + + // 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>(); + 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; + + 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}; // 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}; + BLA::Matrix<1,1> baro_prev = {0}; + + + // padLoop vars: + float numLoop; + BLA::Matrix<3, 1> sumAccel; + BLA::Matrix<3, 1> sumMag; + + + float getLastAttProp(); + float getLastAttUpdate(); + float getLastPVProp(); + float getLastPVUpdate(); + + BLA::Matrix<6, 1> getGPSVar(BLA::Matrix<3, 3> dcm_ned2ecef); + + + // //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 + // }; + + + +}; 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