← Back to branch summary

~durga/maus/rel709

« back to all changes in this revision

Viewing changes to tests/cpp_unit/Recon/Kalman/KalmanFilterTest.cc

  • Committer: Durga Rajaram
  • Date: 2013-08-27 04:36:50 UTC
  • mfrom: (659.1.73 rc)
  • Revision ID: durga@fnal.gov-20130827043650-me0hgsbzlzikdoik
Tags: MAUS-v0.7.0
MAUS-v0.7.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/cpp_unit/Recon/Kalman/KalmanFilterTest.cc
 
1
/* This file is part of MAUS: http://micewww.pp.rl.ac.uk:8080/projects/maus
 
2
 *
 
3
 * MAUS is free software: you can redistribute it and/or modify
 
4
 * it under the terms of the GNU General Public License as published by
 
5
 * the Free Software Foundation, either version 3 of the License, or
 
6
 * (at your option) any later version.
 
7
 *
 
8
 * MAUS is distributed in the hope that it will be useful,
 
9
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 
10
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 
11
 * GNU General Public License for more details.
 
12
 *
 
13
 * You should have received a copy of the GNU General Public License
 
14
 * along with MAUS.  If not, see <http://www.gnu.org/licenses/>.
 
15
 *
 
16
 */
 
17
 
 
18
#include <stdlib.h>
 
19
 
 
20
#include "src/common_cpp/Recon/Kalman/KalmanFilter.hh"
 
21
#include "src/common_cpp/Recon/Kalman/KalmanHelicalPropagator.hh"
 
22
#include "src/common_cpp/Recon/Kalman/KalmanState.hh"
 
23
 
 
24
#include "gtest/gtest.h"
 
25
 
 
26
namespace MAUS {
 
27
 
 
28
class KalmanFilterTest : public ::testing::Test {
 
29
 protected:
 
30
  KalmanFilterTest()  {}
 
31
  virtual ~KalmanFilterTest() {}
 
32
  virtual void SetUp()    {
 
33
    _Bz = 4;
 
34
 
 
35
    old_site.Initialise(5);
 
36
    new_site.Initialise(5);
 
37
    // Old site.
 
38
    old_site.set_id(8);
 
39
    old_site.set_z(-451.132);
 
40
    old_site.set_measurement(-15.5);
 
41
    // New site.
 
42
    new_site.set_id(9);
 
43
    new_site.set_z(-749.828);
 
44
    new_site.set_measurement(-28.5);
 
45
 
 
46
    double px = -6.65702;
 
47
    double py = 20.6712;
 
48
    double pz = 209.21;
 
49
    TMatrixD a(5, 1);
 
50
    a(0, 0) = 52.9;
 
51
    a(1, 0) = px/pz;
 
52
    a(2, 0) = 57.55;
 
53
    a(3, 0) = py/pz;
 
54
    a(4, 0) = 1./pz;
 
55
    old_site.set_a(a, MAUS::KalmanState::Filtered);
 
56
    old_site.set_a(a, MAUS::KalmanState::Projected);
 
57
 
 
58
    TMatrixD C(5, 5);
 
59
    C.Zero();
 
60
    for ( int i = 0; i < 5; ++i ) {
 
61
      C(i, i) = 1.; // dummy values
 
62
    }
 
63
    old_site.set_covariance_matrix(C, MAUS::KalmanState::Projected);
 
64
 
 
65
    kalman_sites.push_back(old_site);
 
66
    kalman_sites.push_back(new_site);
 
67
  }
 
68
  virtual void TearDown() {}
 
69
  MAUS::KalmanState old_site;
 
70
  MAUS::KalmanState new_site;
 
71
  double _Bz;
 
72
  static const int dim = 5;
 
73
  std::vector<MAUS::KalmanState> kalman_sites;
 
74
};
 
75
 
 
76
TEST_F(KalmanFilterTest, test_constructor) {
 
77
  MAUS::KalmanFilter *filter = new MAUS::KalmanFilter(dim);
 
78
  // EXPECT_EQ(track->ndf(), 0.);
 
79
  // EXPECT_EQ(track->tracker(), -1);
 
80
  delete filter;
 
81
}
 
82
 
 
83
/*
 
84
TEST_F(KalmanFilterTest, test_update_H_for_misalignments) {
 
85
  MAUS::KalmanFilter *filter = new MAUS::KalmanFilter(dim);
 
86
  ThreeVector direction_plane0(0., 1., 0.);
 
87
  ThreeVector direction_plane1(0.866, -0.5, 0.0);
 
88
  ThreeVector direction_plane2(-0.866, -0.5, 0.0);
 
89
 
 
90
  MAUS::KalmanState *a_site= new MAUS::KalmanState();
 
91
  a_site->Initialise(dim);
 
92
  a_site->set_direction(direction_plane0);
 
93
 
 
94
  // Say we have a measurement...
 
95
  TMatrixD measurement(2, 1);
 
96
  measurement(0, 0) = 10.;
 
97
  measurement(1, 0) = 0.;
 
98
 
 
99
  // and there's a x,y position corresponding to that measurement...
 
100
  TMatrixD a;
 
101
  a.ResizeTo(5, 1);
 
102
  a(0, 0) = -measurement(0, 0)*1.4945;
 
103
  a(1, 0) = 1.;
 
104
  a(2, 0) = 5.; // random number, y doesnt matter.
 
105
  a(3, 0) = 1.;
 
106
  a(4, 0) = 1.;
 
107
  a_site->set_a(a, MAUS::KalmanState::Projected);
 
108
 
 
109
  TMatrixD s(3, 1);
 
110
  s.Zero();
 
111
 
 
112
  filter->UpdateH(a_site);
 
113
 
 
114
  TMatrixD HA = filter->SolveMeasurementEquation(a, s);
 
115
  EXPECT_NEAR(measurement(0, 0), HA(0, 0), 1e-6);
 
116
 
 
117
  // now, we introduce a shift in x.
 
118
  // The state vector is the same, the measurement is slightly different
 
119
  double shift_x = 2.; // mm
 
120
  s(0, 0) = shift_x; // mm
 
121
  // the new measurement includes the misalignment shift
 
122
  double new_alpha = measurement(0, 0) + shift_x/1.4945;
 
123
 
 
124
  TMatrixD HA_new = filter->SolveMeasurementEquation(a, s);
 
125
 
 
126
  EXPECT_NEAR(new_alpha, HA_new(0, 0), 1e-1);
 
127
  // Now, we introduce a shift in both x & y.
 
128
  double shift_y = 2.; // mm
 
129
  s(1, 0) = shift_y; // mm
 
130
  // So we need a plane that's not 0 (because this one is vertical, only measures x)
 
131
  a_site->set_direction(direction_plane1);
 
132
  ThreeVector perp = direction_plane1.Orthogonal();
 
133
 
 
134
  delete a_site;
 
135
  delete filter;
 
136
}
 
137
 
 
138
TEST_F(KalmanFilterTest, test_filtering_methods) {
 
139
  MAUS::KalmanFilter *filter = new MAUS::KalmanFilter(dim);
 
140
 
 
141
  ThreeVector direction_plane0(0., 1., 0.);
 
142
  ThreeVector direction_plane1(0.866, -0.5, 0.0);
 
143
  ThreeVector direction_plane2(-0.866, -0.5, 0.0);
 
144
 
 
145
  MAUS::KalmanState *a_site= new MAUS::KalmanState();
 
146
  a_site->Initialise(5);
 
147
 
 
148
  // Plane 0. 1st case.
 
149
  a_site->set_direction(direction_plane2);
 
150
  TMatrixD a;
 
151
  a.ResizeTo(5, 1);
 
152
  a(0, 0) = 50.;
 
153
  a(1, 0) = 1.;
 
154
  a(2, 0) = 5.;
 
155
  a(3, 0) = 1.;
 
156
  a(4, 0) = 1./200.;
 
157
 
 
158
  a_site->set_a(a, MAUS::KalmanState::Projected);
 
159
  TMatrixD s(3, 1);
 
160
  s.Zero();
 
161
  filter->UpdateH(a_site);
 
162
  TMatrixD HA = filter->H()*a; // SolveMeasurementEquation(a, s);
 
163
 
 
164
  EXPECT_TRUE(HA(0, 0) > 0);
 
165
  // 2nd case.
 
166
  a(0, 0) = 2.;
 
167
  a(2, 0) = 60.;
 
168
  a_site->set_a(a, MAUS::KalmanState::Projected);
 
169
  HA = filter->H()*a;
 
170
 
 
171
  EXPECT_TRUE(HA(0, 0) < 0);
 
172
  // 3rd case
 
173
  a(0, 0) = -30.;
 
174
  a(2, 0) = 30.;
 
175
  a_site->set_a(a, MAUS::KalmanState::Projected);
 
176
  HA = filter->H()*a;
 
177
 
 
178
  EXPECT_TRUE(HA(0, 0) < 0);
 
179
  // 4th case
 
180
  a(0, 0) = -50.;
 
181
  a(2, 0) = -5.;
 
182
  a_site->set_a(a, MAUS::KalmanState::Projected);
 
183
  HA = filter->H()*a;
 
184
 
 
185
  EXPECT_TRUE(HA(0, 0) < 0);
 
186
  // 5th case
 
187
  a(0, 0) = -2.;
 
188
  a(2, 0) = -60.;
 
189
  a_site->set_a(a, MAUS::KalmanState::Projected);
 
190
  HA = filter->H()*a;
 
191
  EXPECT_TRUE(HA(0, 0) > 0);
 
192
  // 6th case
 
193
  a(0, 0) = 30.;
 
194
  a(2, 0) = -30.;
 
195
  a_site->set_a(a, MAUS::KalmanState::Projected);
 
196
  HA = filter->H()*a;
 
197
 
 
198
  EXPECT_TRUE(HA(0, 0) > 0);
 
199
 
 
200
  a_site->set_measurement(HA(0, 0));
 
201
 
 
202
  // So we have H ready. Let's test the built of W.
 
203
  // For that, we will need V.
 
204
  filter->UpdateV(a_site);
 
205
  TMatrixD C(5, 5);
 
206
  TMatrixD C_S(3, 3);
 
207
  a_site->set_covariance_matrix(C, KalmanState::Projected);
 
208
  a_site->set_input_shift_covariance(C_S);
 
209
 
 
210
  // this breaks.
 
211
  // EXPECT_THROW(track->UpdateW(a_site), Squeal);
 
212
  delete a_site;
 
213
  delete filter;
 
214
}
 
215
*/
 
216
 
 
217
} // ~namespace MAUS