You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
19 lines
521 B
19 lines
521 B
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
#pragma once |
|
|
|
class PosVelEKF { |
|
public: |
|
void init(float pos, float posVar, float vel, float velVar); |
|
void predict(float dt, float dVel, float dVelNoise); |
|
void fusePos(float pos, float posVar); |
|
void fuseVel(float vel, float velVar); |
|
|
|
float getPos() const { return _state[0]; } |
|
float getVel() const { return _state[1]; } |
|
|
|
float getPosNIS(float pos, float posVar); |
|
|
|
private: |
|
float _state[2]; |
|
float _cov[3]; |
|
};
|
|
|