Public Member Functions | Private Attributes | List of all members
EKFilter Class Reference

#include <EKFilter.h>

Public Member Functions

Eigen::Vector3d calcAcc (const Eigen::Vector4d &q) const
 
void calcF (hrp::Matrix77 &F, const Eigen::Vector4d &q, const Eigen::Vector3d &gyro, const Eigen::Vector3d &drift) const
 
void calcH (Eigen::Matrix< double, 3, 7 > &H, const Eigen::Vector4d &q) const
 
Eigen::Vector3d calcMeasurementResidual (const Eigen::Vector3d &acc_measured, const Eigen::Vector4d &q) const
 
void calcOmega (Eigen::Matrix4d &omega, const Eigen::Vector3d &w) const
 
void calcPredictedCovariance (hrp::Matrix77 &_P_a_priori, const hrp::Matrix77 &F, const Eigen::Vector4d &q) const
 
void calcPredictedState (hrp::Vector7 &_x_a_priori, const Eigen::Vector4d &q, const Eigen::Vector3d &gyro, const Eigen::Vector3d &drift) const
 
void calcRWithFuzzyRule (Eigen::Matrix3d &fuzzyR, const hrp::Vector3 &acc, const hrp::Vector3 &gyro) const
 
void correction (const Eigen::Vector3d &z, const Eigen::Matrix3d &fuzzyR)
 
 EKFilter ()
 
hrp::Vector7 getx () const
 
void main_one (hrp::Vector3 &rpy, hrp::Vector3 &rpyRaw, const hrp::Vector3 &acc, const hrp::Vector3 &gyro)
 
void prediction (const Eigen::Vector3d &u)
 
void printAll () const
 
void resetKalmanFilterState ()
 
void setdt (const double _dt)
 

Private Attributes

double dt
 
Eigen::Vector3d g_vec
 
double max_mag_thre_acc
 
double max_mag_thre_gyro
 
double min_mag_thre_acc
 
double min_mag_thre_gyro
 
hrp::Matrix77 P
 
hrp::Matrix77 P_a_priori
 
Eigen::Matrix3d Q
 
Eigen::Matrix3d R
 
hrp::Vector7 x
 
hrp::Vector7 x_a_priori
 
Eigen::Vector3d z_k
 

Detailed Description

Definition at line 14 of file EKFilter.h.

Constructor & Destructor Documentation

◆ EKFilter()

EKFilter::EKFilter ( )
inline

Definition at line 16 of file EKFilter.h.

Member Function Documentation

◆ calcAcc()

Eigen::Vector3d EKFilter::calcAcc ( const Eigen::Vector4d &  q) const
inline

Definition at line 86 of file EKFilter.h.

◆ calcF()

void EKFilter::calcF ( hrp::Matrix77 F,
const Eigen::Vector4d &  q,
const Eigen::Vector3d &  gyro,
const Eigen::Vector3d &  drift 
) const
inline

Definition at line 53 of file EKFilter.h.

◆ calcH()

void EKFilter::calcH ( Eigen::Matrix< double, 3, 7 > &  H,
const Eigen::Vector4d &  q 
) const
inline

Definition at line 92 of file EKFilter.h.

◆ calcMeasurementResidual()

Eigen::Vector3d EKFilter::calcMeasurementResidual ( const Eigen::Vector3d &  acc_measured,
const Eigen::Vector4d &  q 
) const
inline

Definition at line 101 of file EKFilter.h.

◆ calcOmega()

void EKFilter::calcOmega ( Eigen::Matrix4d &  omega,
const Eigen::Vector3d &  w 
) const
inline

Definition at line 30 of file EKFilter.h.

◆ calcPredictedCovariance()

void EKFilter::calcPredictedCovariance ( hrp::Matrix77 _P_a_priori,
const hrp::Matrix77 F,
const Eigen::Vector4d &  q 
) const
inline

Definition at line 70 of file EKFilter.h.

◆ calcPredictedState()

void EKFilter::calcPredictedState ( hrp::Vector7 _x_a_priori,
const Eigen::Vector4d &  q,
const Eigen::Vector3d &  gyro,
const Eigen::Vector3d &  drift 
) const
inline

Definition at line 39 of file EKFilter.h.

◆ calcRWithFuzzyRule()

void EKFilter::calcRWithFuzzyRule ( Eigen::Matrix3d &  fuzzyR,
const hrp::Vector3 acc,
const hrp::Vector3 gyro 
) const
inline

Definition at line 147 of file EKFilter.h.

◆ correction()

void EKFilter::correction ( const Eigen::Vector3d &  z,
const Eigen::Matrix3d &  fuzzyR 
)
inline

Definition at line 122 of file EKFilter.h.

◆ getx()

hrp::Vector7 EKFilter::getx ( ) const
inline

Definition at line 28 of file EKFilter.h.

◆ main_one()

void EKFilter::main_one ( hrp::Vector3 rpy,
hrp::Vector3 rpyRaw,
const hrp::Vector3 acc,
const hrp::Vector3 gyro 
)
inline

Definition at line 162 of file EKFilter.h.

◆ prediction()

void EKFilter::prediction ( const Eigen::Vector3d &  u)
inline

Definition at line 109 of file EKFilter.h.

◆ printAll()

void EKFilter::printAll ( ) const
inline

Definition at line 136 of file EKFilter.h.

◆ resetKalmanFilterState()

void EKFilter::resetKalmanFilterState ( )
inline

Definition at line 174 of file EKFilter.h.

◆ setdt()

void EKFilter::setdt ( const double  _dt)
inline

Definition at line 173 of file EKFilter.h.

Member Data Documentation

◆ dt

double EKFilter::dt
private

Definition at line 184 of file EKFilter.h.

◆ g_vec

Eigen::Vector3d EKFilter::g_vec
private

Definition at line 183 of file EKFilter.h.

◆ max_mag_thre_acc

double EKFilter::max_mag_thre_acc
private

Definition at line 185 of file EKFilter.h.

◆ max_mag_thre_gyro

double EKFilter::max_mag_thre_gyro
private

Definition at line 185 of file EKFilter.h.

◆ min_mag_thre_acc

double EKFilter::min_mag_thre_acc
private

Definition at line 185 of file EKFilter.h.

◆ min_mag_thre_gyro

double EKFilter::min_mag_thre_gyro
private

Definition at line 185 of file EKFilter.h.

◆ P

hrp::Matrix77 EKFilter::P
private

Definition at line 181 of file EKFilter.h.

◆ P_a_priori

hrp::Matrix77 EKFilter::P_a_priori
private

Definition at line 181 of file EKFilter.h.

◆ Q

Eigen::Matrix3d EKFilter::Q
private

Definition at line 182 of file EKFilter.h.

◆ R

Eigen::Matrix3d EKFilter::R
private

Definition at line 182 of file EKFilter.h.

◆ x

hrp::Vector7 EKFilter::x
private

Definition at line 178 of file EKFilter.h.

◆ x_a_priori

hrp::Vector7 EKFilter::x_a_priori
private

Definition at line 178 of file EKFilter.h.

◆ z_k

Eigen::Vector3d EKFilter::z_k
private

Definition at line 183 of file EKFilter.h.


The documentation for this class was generated from the following file:


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:21