mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-08 23:22:04 +08:00
Willem Melching
197c1d92fc
* start cleanup * create generated dir if not exist * tests pass! * everything works again * also convert live_kf to new structure * Remove sympy helpers from file list * Add laika to docker container * Only build models that are present old-commit-hash: 47fd50ca60da419b34469dbc293f1a7f0d3d46a9
38 lines
1.0 KiB
C++
38 lines
1.0 KiB
C++
#pragma once
|
|
|
|
#include <Eigen/Dense>
|
|
#include "cereal/gen/cpp/log.capnp.h"
|
|
|
|
#define DEGREES_TO_RADIANS 0.017453292519943295
|
|
|
|
class Localizer
|
|
{
|
|
Eigen::Matrix2d A;
|
|
Eigen::Matrix2d I;
|
|
Eigen::Matrix2d Q;
|
|
Eigen::Matrix<double, 1, 2> C_posenet;
|
|
Eigen::Matrix<double, 1, 2> C_gyro;
|
|
|
|
double R_gyro;
|
|
|
|
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
|
|
void handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time);
|
|
void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time);
|
|
void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time);
|
|
|
|
public:
|
|
Eigen::Vector2d x;
|
|
Eigen::Matrix2d P;
|
|
double steering_angle = 0;
|
|
double car_speed = 0;
|
|
double posenet_speed = 0;
|
|
double prev_update_time = -1;
|
|
double controls_state_time = -1;
|
|
double sensor_data_time = -1;
|
|
double camera_odometry_time = -1;
|
|
|
|
Localizer();
|
|
void handle_log(cereal::Event::Reader event);
|
|
|
|
};
|