Cansat
/
ESP32-CAN
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

change values. add comment to explain tau. use trapezoidal integration for velocity, in addition to it being used for position

This commit is contained in:
franchioping 2026-04-03 18:20:55 +01:00
parent d5cde816bb
commit c40564dd6d
1 changed files with 16 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
main/sens_fus.h
View File

@ -24,38 +24,42 @@ inline float getYawDifference(const Eigen::Vector3f &v_gps,
struct sens_fus_compl {
Eigen::Vector3f position = Eigen::Vector3f::Zero();
Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
Eigen::Vector3f last_accel_world = Eigen::Vector3f::Zero();
float yaw_offset = 0.0f;
// Time Constants per axis (X, Y, Z)
// Lower = faster tracking of GPS; Higher = smoother/more IMU trust
Eigen::Vector3f tau_gps_pos = {0.5f, 0.5f, 0.5f};
Eigen::Vector3f tau_gps_vel = {1.0f, 1.0f, INFINITY};
/*
* Tau is the time that the filter takes to reach 1-e^(-1) of the difference
* to a constant target
* so at t=tau, were 63% of the way there
* at t=3*tau, were 95% of the way there
*/
Eigen::Vector3f tau_gps_pos = {4.0f, 4.0f, 10.0f};
Eigen::Vector3f tau_gps_vel = {5.0f, 5.0f, INFINITY};
Eigen::Vector3f tau_baro_pos = {INFINITY, INFINITY, 5.0f};
Eigen::Vector3f tau_baro_vel = {INFINITY, INFINITY, 10.0f};
float tau_yaw = 2.0f; // Yaw remains a scalar
float tau_yaw = 10.0f; // Yaw remains a scalar
void predict(float dt, Eigen::Vector3f accel) {
// Rotate body-frame accel to world-frame
Eigen::Vector3f accel_rotated =
Eigen::AngleAxisf(this->yaw_offset, Eigen::Vector3f::UnitZ()) * accel;
Eigen::Vector3f next_velocity = this->velocity + (accel_rotated * dt);
Eigen::Vector3f next_velocity =
this->velocity + (this->last_accel_world + accel_rotated) * 0.5f * dt;
// Trapezoidal integration for position
this->position += (this->velocity + next_velocity) * 0.5f * dt;
this->velocity = next_velocity;
this->last_accel_world = accel_rotated;
}
void measure_gps(float dt, Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
// Calculate Alpha vectors using element-wise operations
// Formula: alpha = dt / (tau + dt)
// alpha = dt / (tau + dt)
Eigen::Vector3f alpha_pos = dt / (tau_gps_pos.array() + dt);
Eigen::Vector3f alpha_vel = dt / (tau_gps_vel.array() + dt);
float alpha_yaw = dt / (tau_yaw + dt);
// 1. Position Update (Element-wise LPF)
// res = (1 - alpha) * state + alpha * measurement
this->position =
(Eigen::Vector3f::Ones() - alpha_pos).array() * this->position.array() +
@ -70,7 +74,7 @@ struct sens_fus_compl {
std::atan2(std::sin(this->yaw_offset), std::cos(this->yaw_offset));
}
// 3. Velocity Update (Element-wise LPF)
// res = (1 - alpha) * state + alpha * measurement
this->velocity =
(Eigen::Vector3f::Ones() - alpha_vel).array() * this->velocity.array() +
alpha_vel.array() * gps_vel.array();
@ -78,7 +82,6 @@ struct sens_fus_compl {
void measure_baro(float dt, Eigen::Vector3f baro_pos,
Eigen::Vector3f baro_vel) {
// Calculate Alpha vectors using element-wise operations
// Formula: alpha = dt / (tau + dt)
Eigen::Vector3f alpha_pos = dt / (tau_baro_pos.array() + dt);
Eigen::Vector3f alpha_vel = dt / (tau_baro_vel.array() + dt);