12 #ifndef __SOT_KALMAN_H 13 #define __SOT_KALMAN_H 19 #include <dynamic-graph/all-signals.h> 20 #include <dynamic-graph/entity.h> 21 #include <dynamic-graph/linear-algebra.h> 30 #if defined(kalman_EXPORTS) 31 #define SOT_KALMAN_EXPORT __declspec(dllexport) 33 #define SOT_KALMAN_EXPORT __declspec(dllimport) 36 #define SOT_KALMAN_EXPORT 49 virtual const std::string &
getClassName(
void)
const {
return CLASS_NAME; }
73 return "Implementation of extended Kalman filter \n" 75 " Dynamics of the system: \n" 77 " x = f (x , u ) + w (state) \n" 80 " y = h (x ) + v (observation)\n" 86 " x = f (x , u ) (state) \n" 87 " k|k-1 k-1|k-1 k-1 \n" 90 " P = F P F + Q (covariance)\n" 91 " k|k-1 k-1 k-1|k-1 k-1 \n" 96 " F = --- (x , u ) \n" 97 " k-1 \\ k-1|k-1 k-1 \n" 109 " z = y - h (x ) (innovation)\n" 112 " S = H P H + R (innovation covariance)\n" 115 " K = P H S (Kalman gain)\n" 118 " x = x + K z (state) \n" 121 " P =(I - K H ) P \n" 125 " - input(vector)::x_pred: state prediction\n" 127 " - input(vector)::y_pred: observation prediction: h (x )\n" 129 " - input(matrix)::F: partial derivative wrt x of f\n" 130 " - input(vector)::y: measure \n" 131 " - input(matrix)::H: partial derivative wrt x of h\n" 132 " - input(matrix)::Q: variance of noise w\n" 134 " - input(matrix)::R: variance of noise v\n" 136 " - output(matrix)::P_pred: variance of prediction\n" 138 " - output(vector)::x_est: state estimation x\n" 143 Matrix &computeVarianceUpdate(Matrix &P_k_k,
const int &time);
144 Vector &computeStateUpdate(Vector &x_est,
const int &time);
147 stateEstimation_ = x0;
148 stateUpdateSOUT.recompute(0);
153 varianceUpdateSOUT.recompute(0);
186 Kalman(
const std::string &name);
188 void display(std::ostream &os)
const;
Matrix stateVariance_
Definition: kalman.hh:163
void setStateVariance(const Matrix &P0)
Definition: kalman.hh:151
SignalPtr< Vector, int > statePredictedSIN
Definition: kalman.hh:63
Matrix K_
Definition: kalman.hh:183
SignalPtr< Matrix, int > modelMeasureSIN
Definition: kalman.hh:59
Matrix S_
Definition: kalman.hh:180
double dt
Definition: kalman.hh:54
SignalPtr< Matrix, int > noiseTransitionSIN
Definition: kalman.hh:60
virtual const std::string & getClassName(void) const
Definition: kalman.hh:49
SignalTimeDependent< Matrix, int > gainSINTERN
Definition: kalman.hh:68
SignalPtr< Matrix, int > modelTransitionSIN
Definition: kalman.hh:58
Vector stateEstimation_
Definition: kalman.hh:159
static const std::string CLASS_NAME
Definition: kalman.hh:48
SignalTimeDependent< Matrix, int > varianceUpdateSOUT
Definition: kalman.hh:65
Matrix Pk_k_1_
Definition: kalman.hh:177
unsigned int size_state
Definition: kalman.hh:52
SignalPtr< Vector, int > observationPredictedSIN
Definition: kalman.hh:64
SignalTimeDependent< Vector, int > stateUpdateSOUT
Definition: kalman.hh:66
unsigned int size_measure
Definition: kalman.hh:53
#define SOT_KALMAN_EXPORT
Definition: kalman.hh:36
SignalTimeDependent< Matrix, int > innovationSINTERN
Definition: kalman.hh:69
virtual std::string getDocString() const
Definition: kalman.hh:72
SignalPtr< Matrix, int > noiseMeasureSIN
Definition: kalman.hh:61
Matrix FP_
Definition: kalman.hh:172
void setStateEstimation(const Vector &x0)
Definition: kalman.hh:146
SignalPtr< Vector, int > measureSIN
Definition: kalman.hh:57
Definition: abstract-sot-external-interface.hh:17
Vector z_
Definition: kalman.hh:168