28 lines
954 B
C++
28 lines
954 B
C++
#include <Eigen/Dense>
|
|
|
|
struct nav_compl {
|
|
Eigen::Vector3f position = Eigen::Vector3f::Zero();
|
|
Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
|
|
|
|
Eigen::Vector3f gps_weight_vel = Eigen::Vector3f::Constant(0.01f);
|
|
Eigen::Vector3f gps_weight_pos = Eigen::Vector3f::Constant(0.01f);
|
|
|
|
void predict(float dt, Eigen::Vector3f accel) {
|
|
Eigen::Vector3f next_velocity = this->velocity + (accel * dt);
|
|
|
|
this->position += (this->velocity + next_velocity) * 0.5f * dt;
|
|
|
|
this->velocity = next_velocity;
|
|
}
|
|
|
|
void measure(Eigen::Vector3f gps_pos, Eigen::Vector3f gps_vel) {
|
|
this->position = (Eigen::Vector3f::Ones() - gps_weight_pos)
|
|
.cwiseProduct(this->position) +
|
|
gps_weight_pos.cwiseProduct(gps_pos);
|
|
|
|
this->velocity = (Eigen::Vector3f::Ones() - gps_weight_vel)
|
|
.cwiseProduct(this->velocity) +
|
|
gps_weight_vel.cwiseProduct(gps_vel);
|
|
}
|
|
};
|