GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/tools/kalman.cpp Lines: 0 61 0.0 %
Date: 2023-03-13 12:09:37 Branches: 0 166 0.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2010,
3
 * Nicolas Mansard,
4
 * François Bleibel,
5
 * Olivier Stasse,
6
 * Florent Lamiraux
7
 *
8
 * CNRS/AIST
9
 *
10
 */
11
12
/* --- SOT --- */
13
#include <dynamic-graph/command-setter.h>
14
#include <dynamic-graph/factory.h>
15
16
#include <sot/core/debug.hh>
17
#include <sot/core/exception-tools.hh>
18
#include <sot/core/factory.hh>
19
#include <sot/core/kalman.hh> /* Header of the class implemented here.   */
20
21
namespace dynamicgraph {
22
using command::Setter;
23
namespace sot {
24
25
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Kalman, "Kalman");
26
27
Kalman::Kalman(const std::string &name)
28
    : Entity(name),
29
      measureSIN(NULL, "Kalman(" + name + ")::input(vector)::y"),
30
      modelTransitionSIN(NULL, "Kalman(" + name + ")::input(matrix)::F"),
31
      modelMeasureSIN(NULL, "Kalman(" + name + ")::input(matrix)::H"),
32
      noiseTransitionSIN(NULL, "Kalman(" + name + ")::input(matrix)::Q"),
33
      noiseMeasureSIN(NULL, "Kalman(" + name + ")::input(matrix)::R")
34
35
      ,
36
      statePredictedSIN(0, "Kalman(" + name + ")::input(vector)::x_pred"),
37
      observationPredictedSIN(0, "Kalman(" + name + ")::input(vector)::y_pred"),
38
      varianceUpdateSOUT("Kalman(" + name + ")::output(vector)::P"),
39
      stateUpdateSOUT("Kalman(" + name + ")::output(vector)::x_est"),
40
      stateEstimation_(),
41
      stateVariance_() {
42
  sotDEBUGIN(15);
43
  varianceUpdateSOUT.setFunction(
44
      boost::bind(&Kalman::computeVarianceUpdate, this, _1, _2));
45
  stateUpdateSOUT.setFunction(
46
      boost::bind(&Kalman::computeStateUpdate, this, _1, _2));
47
48
  signalRegistration(measureSIN << observationPredictedSIN << modelTransitionSIN
49
                                << modelMeasureSIN << noiseTransitionSIN
50
                                << noiseMeasureSIN << statePredictedSIN
51
                                << stateUpdateSOUT << varianceUpdateSOUT);
52
53
  std::string docstring =
54
      "  Set initial state estimation\n"
55
      "\n"
56
      "  input:\n"
57
      "    - a vector: initial state\n";
58
  addCommand("setInitialState",
59
             new Setter<Kalman, Vector>(*this, &Kalman::setStateEstimation,
60
                                        docstring));
61
62
  docstring =
63
      "  Set variance of initial state estimation\n"
64
      "\n"
65
      "  input:\n"
66
      "    - a matrix: variance covariance matrix\n";
67
  addCommand(
68
      "setInitialVariance",
69
      new Setter<Kalman, Matrix>(*this, &Kalman::setStateVariance, docstring));
70
  sotDEBUGOUT(15);
71
}
72
73
Matrix &Kalman::computeVarianceUpdate(Matrix &Pk_k, const int &time) {
74
  sotDEBUGIN(15);
75
  if (time == 0) {
76
    // First time return variance initial state
77
    Pk_k = stateVariance_;
78
    // Set dependency to input signals for latter computations
79
    varianceUpdateSOUT.addDependency(noiseTransitionSIN);
80
    varianceUpdateSOUT.addDependency(modelTransitionSIN);
81
  } else {
82
    const Matrix &Q = noiseTransitionSIN(time);
83
    const Matrix &R = noiseMeasureSIN(time);
84
    const Matrix &F = modelTransitionSIN(time);
85
    const Matrix &H = modelMeasureSIN(time);
86
    const Matrix &Pk_1_k_1 = stateVariance_;
87
88
    sotDEBUG(15) << "Q=" << Q << std::endl;
89
    sotDEBUG(15) << "R=" << R << std::endl;
90
    sotDEBUG(15) << "F=" << F << std::endl;
91
    sotDEBUG(15) << "H=" << H << std::endl;
92
    sotDEBUG(15) << "Pk_1_k_1=" << Pk_1_k_1 << std::endl;
93
94
    FP_ = F * Pk_1_k_1;
95
    Pk_k_1_ = FP_ * F.transpose();
96
    Pk_k_1_ += Q;
97
98
    sotDEBUG(15) << "F " << std::endl << F << std::endl;
99
    sotDEBUG(15) << "P_{k-1|k-1} " << std::endl << Pk_1_k_1 << std::endl;
100
    sotDEBUG(15) << "F^T " << std::endl << F.transpose() << std::endl;
101
    sotDEBUG(15) << "P_{k|k-1} " << std::endl << Pk_k_1_ << std::endl;
102
103
    S_ = H * Pk_k_1_ * H.transpose() + R;
104
    K_ = Pk_k_1_ * H.transpose() * S_.inverse();
105
    Pk_k = Pk_k_1_ - K_ * H * Pk_k_1_;
106
107
    sotDEBUG(15) << "S_{k} " << std::endl << S_ << std::endl;
108
    sotDEBUG(15) << "K_{k} " << std::endl << K_ << std::endl;
109
    sotDEBUG(15) << "P_{k|k} " << std::endl << Pk_k << std::endl;
110
111
    sotDEBUGOUT(15);
112
113
    stateVariance_ = Pk_k;
114
  }
115
  return Pk_k;
116
}
117
118
//               ^
119
//   z = y  - h (x     )
120
//    k   k       k|k-1
121
//
122
//   ^     ^
123
//   x   = x      + K  z
124
//    k|k   k|k-1    k  k
125
//
126
//                  T
127
//   S = H  P      H  + R
128
//    k   k  k|k-1  k
129
//
130
//               T  -1
131
//   K = P      H  S
132
//    k   k|k-1  k  k
133
//
134
//   P   = (I - K  H ) P
135
//    k|k        k  k   k|k-1
136
137
Vector &Kalman::computeStateUpdate(Vector &x_est, const int &time) {
138
  sotDEBUGIN(15);
139
  if (time == 0) {
140
    // First time return variance initial state
141
    x_est = stateEstimation_;
142
    // Set dependency to input signals for latter computations
143
    stateUpdateSOUT.addDependency(measureSIN);
144
    stateUpdateSOUT.addDependency(observationPredictedSIN);
145
    stateUpdateSOUT.addDependency(modelMeasureSIN);
146
    stateUpdateSOUT.addDependency(noiseTransitionSIN);
147
    stateUpdateSOUT.addDependency(noiseMeasureSIN);
148
    stateUpdateSOUT.addDependency(statePredictedSIN);
149
    stateUpdateSOUT.addDependency(varianceUpdateSOUT);
150
  } else {
151
    varianceUpdateSOUT.recompute(time);
152
    const Vector &x_pred = statePredictedSIN(time);
153
    const Vector &y_pred = observationPredictedSIN(time);
154
    const Vector &y = measureSIN(time);
155
156
    sotDEBUG(25) << "K_{k} = " << std::endl << K_ << std::endl;
157
    sotDEBUG(25) << "y = " << y << std::endl;
158
    sotDEBUG(25) << "h (\\hat{x}_{k|k-1}) = " << y_pred << std::endl;
159
    sotDEBUG(25) << "y = " << y << std::endl;
160
161
    // Innovation: z_ = y - Hx
162
    z_ = y - y_pred;
163
    // x_est = x_pred + (K*(y-(H*x_pred)));
164
    x_est = x_pred + K_ * z_;
165
166
    sotDEBUG(25) << "z_{k} = " << z_ << std::endl;
167
    sotDEBUG(25) << "x_{k|k} = " << x_est << std::endl;
168
169
    stateEstimation_ = x_est;
170
  }
171
  sotDEBUGOUT(15);
172
  return x_est;
173
}
174
175
/* -------------------------------------------------------------------------- */
176
/* --- MODELE --------------------------------------------------------------- */
177
/* -------------------------------------------------------------------------- */
178
179
void Kalman::display(std::ostream &) const {}
180
181
}  // namespace sot
182
}  // namespace dynamicgraph
183
184
/*!
185
  \file Kalman.cpp
186
  \brief  Generic kalman filtering implementation
187
*/