12 #ifndef __SOT_KALMAN_H 13 #define __SOT_KALMAN_H 20 #include <dynamic-graph/all-signals.h> 21 #include <dynamic-graph/entity.h> 22 #include <dynamic-graph/linear-algebra.h> 29 #if defined(kalman_EXPORTS) 30 #define SOT_KALMAN_EXPORT __declspec(dllexport) 32 #define SOT_KALMAN_EXPORT __declspec(dllimport) 35 #define SOT_KALMAN_EXPORT 48 virtual const std::string &
getClassName(
void)
const {
return CLASS_NAME; }
72 return "Implementation of extended Kalman filter \n" 74 " Dynamics of the system: \n" 76 " x = f (x , u ) + w (state) \n" 79 " y = h (x ) + v (observation)\n" 85 " x = f (x , u ) (state) \n" 86 " k|k-1 k-1|k-1 k-1 \n" 89 " P = F P F + Q (covariance)\n" 90 " k|k-1 k-1 k-1|k-1 k-1 \n" 95 " F = --- (x , u ) \n" 96 " k-1 \\ k-1|k-1 k-1 \n" 108 " z = y - h (x ) (innovation)\n" 111 " S = H P H + R (innovation covariance)\n" 114 " K = P H S (Kalman gain)\n" 117 " x = x + K z (state) \n" 120 " P =(I - K H ) P \n" 124 " - input(vector)::x_pred: state prediction\n" 126 " - input(vector)::y_pred: observation prediction: h (x )\n" 128 " - input(matrix)::F: partial derivative wrt x of f\n" 129 " - input(vector)::y: measure \n" 130 " - input(matrix)::H: partial derivative wrt x of h\n" 131 " - input(matrix)::Q: variance of noise w\n" 133 " - input(matrix)::R: variance of noise v\n" 135 " - output(matrix)::P_pred: variance of prediction\n" 137 " - output(vector)::x_est: state estimation x\n" 142 Matrix &computeVarianceUpdate(Matrix &P_k_k,
const int &time);
143 Vector &computeStateUpdate(Vector &x_est,
const int &time);
146 stateEstimation_ = x0;
147 stateUpdateSOUT.recompute(0);
152 varianceUpdateSOUT.recompute(0);
185 Kalman(
const std::string &name);
187 void display(std::ostream &os)
const;
Matrix stateVariance_
Definition: kalman.hh:162
void setStateVariance(const Matrix &P0)
Definition: kalman.hh:150
SignalPtr< Vector, int > statePredictedSIN
Definition: kalman.hh:62
Matrix K_
Definition: kalman.hh:182
SignalPtr< Matrix, int > modelMeasureSIN
Definition: kalman.hh:58
Matrix S_
Definition: kalman.hh:179
double dt
Definition: kalman.hh:53
SignalPtr< Matrix, int > noiseTransitionSIN
Definition: kalman.hh:59
virtual const std::string & getClassName(void) const
Definition: kalman.hh:48
SignalTimeDependent< Matrix, int > gainSINTERN
Definition: kalman.hh:67
SignalPtr< Matrix, int > modelTransitionSIN
Definition: kalman.hh:57
Vector stateEstimation_
Definition: kalman.hh:158
static const std::string CLASS_NAME
Definition: kalman.hh:47
SignalTimeDependent< Matrix, int > varianceUpdateSOUT
Definition: kalman.hh:64
Matrix Pk_k_1_
Definition: kalman.hh:176
unsigned int size_state
Definition: kalman.hh:51
SignalPtr< Vector, int > observationPredictedSIN
Definition: kalman.hh:63
SignalTimeDependent< Vector, int > stateUpdateSOUT
Definition: kalman.hh:65
unsigned int size_measure
Definition: kalman.hh:52
#define SOT_KALMAN_EXPORT
Definition: kalman.hh:35
SignalTimeDependent< Matrix, int > innovationSINTERN
Definition: kalman.hh:68
virtual std::string getDocString() const
Definition: kalman.hh:71
SignalPtr< Matrix, int > noiseMeasureSIN
Definition: kalman.hh:60
Matrix FP_
Definition: kalman.hh:171
void setStateEstimation(const Vector &x0)
Definition: kalman.hh:145
SignalPtr< Vector, int > measureSIN
Definition: kalman.hh:56
Definition: abstract-sot-external-interface.hh:17
Vector z_
Definition: kalman.hh:167