iCub-main
kalman.cpp
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 
11 #include <cmath>
12 
13 #include <yarp/math/Math.h>
14 #include <yarp/math/SVD.h>
15 #include <iCub/ctrl/kalman.h>
16 
17 using namespace std;
18 using namespace yarp::sig;
19 using namespace yarp::math;
20 using namespace iCub::ctrl;
21 
22 
23 /**********************************************************************/
24 void Kalman::initialize()
25 {
26  n=A.rows();
27  m=H.rows();
28 
29  At=A.transposed();
30  Ht=H.transposed();
31  I=eye((int)n,(int)n);
32 
33  x.resize(n,0.0);
34  P.resize(n,n); P.zero();
35  K.resize(n,m); K.zero();
36  S.resize(m,m); S.zero();
37  validationGate=0.0;
38 }
39 
40 
41 /**********************************************************************/
42 Kalman::Kalman(const Matrix &_A, const Matrix &_H, const Matrix &_Q,
43  const Matrix &_R) : A(_A), H(_H), Q(_Q), R(_R)
44 {
45  initialize();
46  B.resize(n,n); B.zero();
47 }
48 
49 
50 /**********************************************************************/
51 Kalman::Kalman(const Matrix &_A, const Matrix &_B, const Matrix &_H,
52  const Matrix &_Q, const Matrix &_R) : A(_A), B(_B), H(_H), Q(_Q), R(_R)
53 {
54  initialize();
55 }
56 
57 
58 /**********************************************************************/
59 bool Kalman::init(const Vector &_x0, const Matrix &_P0)
60 {
61  if ((_x0.length()==x.length()) && (_P0.rows()==P.rows()) && (_P0.cols()==P.cols()))
62  {
63  x=_x0;
64  P=_P0;
65  return true;
66  }
67  else
68  return false;
69 }
70 
71 
72 /**********************************************************************/
73 const Vector& Kalman::predict(const Vector &u)
74 {
75  x=A*x+B*u;
76  P=A*P*At+Q;
77  S=H*P*Ht+R;
78  validationGate=0.0;
79  return x;
80 }
81 
82 
83 /**********************************************************************/
84 const Vector& Kalman::predict()
85 {
86  return predict(Vector(n,0.0));
87 }
88 
89 
90 /**********************************************************************/
91 const Vector& Kalman::correct(const Vector &z)
92 {
93  Matrix invS=pinv(S);
94  K=P*Ht*invS;
95  Vector e=z-get_y();
96  x+=K*e;
97  P=(I-K*H)*P;
98  validationGate=yarp::math::dot(e,invS*e);
99  return x;
100 }
101 
102 
103 /**********************************************************************/
104 const Vector& Kalman::filt(const Vector &u, const Vector &z)
105 {
106  predict(u);
107  correct(z);
108  return x;
109 }
110 
111 
112 /**********************************************************************/
113 const Vector& Kalman::filt(const Vector &z)
114 {
115  return filt(Vector(n,0.0),z);
116 }
117 
118 
119 /**********************************************************************/
120 Vector Kalman::get_y() const
121 {
122  return H*x;
123 }
124 
125 
126 /**********************************************************************/
127 bool Kalman::set_A(const yarp::sig::Matrix &_A)
128 {
129  if ((_A.cols()==A.cols()) && (_A.rows()==A.rows()))
130  {
131  A=_A;
132  At=A.transposed();
133  return true;
134  }
135  else
136  return false;
137 }
138 
139 
140 /**********************************************************************/
141 bool Kalman::set_B(const yarp::sig::Matrix &_B)
142 {
143  if ((_B.cols()==B.cols()) && (_B.rows()==B.rows()))
144  {
145  B=_B;
146  return true;
147  }
148  else
149  return false;
150 }
151 
152 
153 /**********************************************************************/
154 bool Kalman::set_H(const yarp::sig::Matrix &_H)
155 {
156  if ((_H.cols()==H.cols()) && (_H.rows()==H.rows()))
157  {
158  H=_H;
159  Ht=H.transposed();
160  return true;
161  }
162  else
163  return false;
164 }
165 
166 
167 /**********************************************************************/
168 bool Kalman::set_Q(const yarp::sig::Matrix &_Q)
169 {
170  if ((_Q.cols()==Q.cols()) && (_Q.rows()==Q.rows()))
171  {
172  Q=_Q;
173  return true;
174  }
175  else
176  return false;
177 }
178 
179 
180 /**********************************************************************/
181 bool Kalman::set_R(const yarp::sig::Matrix &_R)
182 {
183  if ((_R.cols()==R.cols()) && (_R.rows()==R.rows()))
184  {
185  R=_R;
186  return true;
187  }
188  else
189  return false;
190 }
191 
192 
STL namespace.
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the dot product between two vectors given in the form: matrix(:,col).
int n
A
Definition: sine.m:16