ekf.hpp

Go to the documentation of this file.

#pragma once
 
#include "LightLib/drive/mcl_config.hpp"
#include "LightLib/drive/odometry.hpp"
 
namespace light::ekf {
 
void init(const Pose& initial, MCLConfig cfg = {});
 
void reset(const Pose& pose);
 
void predict(float dLocalX, float dLocalY, float dTheta, float dt);
 
void updateHeadingIMU(float thetaRad, float variance);
void updateGPS(float x, float y, float variance);
void updateWheelPose(const Pose& measured, float variance);
Pose mean();
 
Pose velocity();
 
float covTrace();
 
bool diverged(float threshold_in_sq);
 
MCLConfig config();
void setConfig(const MCLConfig& cfg);
}  // namespace light::ekf