iCub-main
Loading...
Searching...
No Matches
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
17using namespace std;
18using namespace yarp::sig;
19using namespace yarp::math;
20using namespace iCub::ctrl;
21
22
23/**********************************************************************/
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();
38}
39
40
41/**********************************************************************/
42Kalman::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/**********************************************************************/
51Kalman::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/**********************************************************************/
59bool 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/**********************************************************************/
73const 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;
79 return x;
80}
81
82
83/**********************************************************************/
84const Vector& Kalman::predict()
85{
86 return predict(Vector(n,0.0));
87}
88
89
90/**********************************************************************/
91const 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/**********************************************************************/
104const Vector& Kalman::filt(const Vector &u, const Vector &z)
105{
106 predict(u);
107 correct(z);
108 return x;
109}
110
111
112/**********************************************************************/
113const Vector& Kalman::filt(const Vector &z)
114{
115 return filt(Vector(n,0.0),z);
116}
117
118
119/**********************************************************************/
120Vector Kalman::get_y() const
121{
122 return H*x;
123}
124
125
126/**********************************************************************/
127bool 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/**********************************************************************/
141bool 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/**********************************************************************/
154bool 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/**********************************************************************/
168bool 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/**********************************************************************/
181bool 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
yarp::sig::Matrix At
Definition kalman.h:44
bool set_Q(const yarp::sig::Matrix &_Q)
Returns the process noise covariance matrix.
Definition kalman.cpp:168
bool set_R(const yarp::sig::Matrix &_R)
Returns the measurement noise covariance matrix.
Definition kalman.cpp:181
yarp::sig::Matrix Ht
Definition kalman.h:45
yarp::sig::Matrix I
Definition kalman.h:49
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 A
Definition kalman.h:44
bool set_A(const yarp::sig::Matrix &_A)
Returns the state transition matrix.
Definition kalman.cpp:127
yarp::sig::Matrix R
Definition kalman.h:48
yarp::sig::Vector get_y() const
Returns the estimated output.
Definition kalman.cpp:120
bool set_B(const yarp::sig::Matrix &_B)
Returns the input matrix.
Definition kalman.cpp:141
yarp::sig::Matrix H
Definition kalman.h:45
double validationGate
Definition kalman.h:55
yarp::sig::Matrix Q
Definition kalman.h:47
const yarp::sig::Vector & predict()
Predicts the next state vector.
Definition kalman.cpp:84
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 S
Definition kalman.h:54
yarp::sig::Matrix B
Definition kalman.h:46
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 ...
yarp::sig::Matrix K
Definition kalman.h:53
yarp::sig::Matrix P
Definition kalman.h:52
yarp::sig::Vector x
Definition kalman.h:51
bool set_H(const yarp::sig::Matrix &_H)
Returns the measurement transition matrix.
Definition kalman.cpp:154
A
Definition sine.m:16