iCub-main
kalman.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
22 #ifndef __KALMAN_H__
23 #define __KALMAN_H__
24 
25 #include <yarp/sig/Vector.h>
26 #include <yarp/sig/Matrix.h>
27 #include <iCub/ctrl/math.h>
28 
29 
30 namespace iCub
31 {
32 
33 namespace ctrl
34 {
35 
41 class Kalman
42 {
43 protected:
44  yarp::sig::Matrix A, At;
45  yarp::sig::Matrix H, Ht;
46  yarp::sig::Matrix B;
47  yarp::sig::Matrix Q;
48  yarp::sig::Matrix R;
49  yarp::sig::Matrix I;
50 
51  yarp::sig::Vector x;
52  yarp::sig::Matrix P;
53  yarp::sig::Matrix K;
54  yarp::sig::Matrix S;
56 
57  size_t n;
58  size_t m;
59 
60  void initialize();
61 
62  // Default constructor: not implemented.
63  Kalman();
64 
65 public:
74  Kalman(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_H,
75  const yarp::sig::Matrix &_Q, const yarp::sig::Matrix &_R);
76 
86  Kalman(const yarp::sig::Matrix &_A, const yarp::sig::Matrix &_B,
87  const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_Q,
88  const yarp::sig::Matrix &_R);
89 
97  bool init(const yarp::sig::Vector &_x0, const yarp::sig::Matrix &_P0);
98 
106  const yarp::sig::Vector& predict(const yarp::sig::Vector &u);
107 
113  const yarp::sig::Vector& predict();
114 
123  const yarp::sig::Vector& correct(const yarp::sig::Vector &z);
124 
135  const yarp::sig::Vector& filt(const yarp::sig::Vector &u, const yarp::sig::Vector &z);
136 
146  const yarp::sig::Vector& filt(const yarp::sig::Vector &z);
147 
153  const yarp::sig::Vector& get_x() const { return x; }
154 
160  yarp::sig::Vector get_y() const;
161 
167  const yarp::sig::Matrix& get_P() const { return P; }
168 
174  const yarp::sig::Matrix& get_S() const { return S; }
175 
183  double get_ValidationGate() const { return validationGate; }
184 
190  const yarp::sig::Matrix& get_K() const { return K; }
191 
197  const yarp::sig::Matrix& get_A() const { return A; }
198 
204  const yarp::sig::Matrix& get_B() const { return B; }
205 
211  const yarp::sig::Matrix& get_H() const { return H; }
212 
218  const yarp::sig::Matrix& get_Q() const { return Q; }
219 
225  const yarp::sig::Matrix& get_R() const { return R; }
226 
233  bool set_A(const yarp::sig::Matrix &_A);
234 
241  bool set_B(const yarp::sig::Matrix &_B);
242 
249  bool set_H(const yarp::sig::Matrix &_H);
250 
257  bool set_Q(const yarp::sig::Matrix &_Q);
258 
265  bool set_R(const yarp::sig::Matrix &_R);
266 };
267 
268 }
269 
270 }
271 
272 #endif
273 
274 
275 
const yarp::sig::Vector & get_x() const
Returns the estimated state.
Definition: kalman.h:153
bool set_A(const yarp::sig::Matrix &_A)
Returns the state transition matrix.
Definition: kalman.cpp:127
const yarp::sig::Vector & predict()
Predicts the next state vector.
Definition: kalman.cpp:84
double get_ValidationGate() const
Returns the validation gate.
Definition: kalman.h:183
yarp::sig::Matrix K
Definition: kalman.h:53
yarp::sig::Matrix S
Definition: kalman.h:54
const yarp::sig::Matrix & get_A() const
Returns the state transition matrix.
Definition: kalman.h:197
yarp::sig::Matrix A
Definition: kalman.h:44
double validationGate
Definition: kalman.h:55
const yarp::sig::Matrix & get_B() const
Returns the input matrix.
Definition: kalman.h:204
const yarp::sig::Matrix & get_H() const
Returns the measurement matrix.
Definition: kalman.h:211
const yarp::sig::Matrix & get_Q() const
Returns the process noise covariance matrix.
Definition: kalman.h:218
yarp::sig::Vector x
Definition: kalman.h:51
yarp::sig::Matrix B
Definition: kalman.h:46
Classic Kalman estimator.
Definition: kalman.h:41
yarp::sig::Vector get_y() const
Returns the estimated output.
Definition: kalman.cpp:120
bool init(const yarp::sig::Vector &_x0, const yarp::sig::Matrix &_P0)
Set initial state and error covariance.
Definition: kalman.cpp:59
yarp::sig::Matrix P
Definition: kalman.h:52
yarp::sig::Matrix H
Definition: kalman.h:45
const yarp::sig::Matrix & get_K() const
Returns the Kalman gain matrix.
Definition: kalman.h:190
yarp::sig::Matrix Ht
Definition: kalman.h:45
bool set_R(const yarp::sig::Matrix &_R)
Returns the measurement noise covariance matrix.
Definition: kalman.cpp:181
const yarp::sig::Vector & correct(const yarp::sig::Vector &z)
Corrects the current estimation of the state vector given the current measurement.
Definition: kalman.cpp:91
yarp::sig::Matrix At
Definition: kalman.h:44
const yarp::sig::Matrix & get_R() const
Returns the measurement noise covariance matrix.
Definition: kalman.h:225
yarp::sig::Matrix Q
Definition: kalman.h:47
const yarp::sig::Matrix & get_S() const
Returns the estimated measurement covariance.
Definition: kalman.h:174
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
bool set_B(const yarp::sig::Matrix &_B)
Returns the input matrix.
Definition: kalman.cpp:141
const yarp::sig::Vector & filt(const yarp::sig::Vector &u, const yarp::sig::Vector &z)
Returns the estimated state vector given the current input and the current measurement by performing ...
void initialize()
Definition: kalman.cpp:24
bool set_H(const yarp::sig::Matrix &_H)
Returns the measurement transition matrix.
Definition: kalman.cpp:154
bool set_Q(const yarp::sig::Matrix &_Q)
Returns the process noise covariance matrix.
Definition: kalman.cpp:168
yarp::sig::Matrix I
Definition: kalman.h:49
yarp::sig::Matrix R
Definition: kalman.h:48
const yarp::sig::Matrix & get_P() const
Returns the estimated state covariance.
Definition: kalman.h:167