mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
20 lines
521 B
C
20 lines
521 B
C
|
/// -*- 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];
|
||
|
|
};
|