mht_types.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
32 #include <mtt/mht_types.h>
33 #error This file should not be used, DEPRECATED
34 
36 
37 void constantVelocityEKFilter::SetIdentity(Matrix& M,int size,double value)
38 {
39  for(int l=0;l<size;l++)
40  for(int c=0;c<size;c++)
41  if(c==l)
42  M(l,c)=value;
43  else
44  M(l,c)=0;
45 }
46 
48 {
49  for(int l=0;l<size;l++)
50  for(int c=0;c<size;c++)
51  M(l,c)=0;
52 }
53 
55 {
56  setDim(4,0,4,2,2);
57  dt=1./50.;
58  lt=Time::now();
59 
60  Vector x_0(4);
61 
62  x_0(0)=x_init(0);//x
63  x_0(1)=x_init(1);//y
64  x_0(2)=x_init(2);//vx
65  x_0(3)=x_init(3);//vy
66 
67  Matrix P_0(4,4);
68 
70 
71  //Large initial uncertainty
72  SetIdentity(P_0,4,2.0);
73 
74  init(x_0,P_0);
75 
76 // cout<<"Real init"<<endl;
77 // cout<<"P_0: "<<P_0<<endl;
78 // cout<<"x_0: "<<x_0<<endl;
79 
80 // cout<<"P: "<<calculateP() <<endl;
81 // cout<<"x: "<<x<<endl;
82 }
83 
84 void constantVelocityEKFilter::InitFilter(Vector4d& x_init,Matrix4d& P_init)
85 {
86  setDim(4,0,4,2,2);
87  dt=1./50.;
88  lt=Time::now();
89 
90  Vector x_0(4);
91 
92  x_0(0)=x_init(0);//x
93  x_0(1)=x_init(1);//y
94  x_0(2)=x_init(2);//vx
95  x_0(3)=x_init(3);//vy
96 
97  Matrix P_0(4,4);
98 
99  P_0(0,0)=P_init(0,0);
100  P_0(0,1)=P_init(0,1);
101  P_0(0,2)=P_init(0,2);
102  P_0(0,3)=P_init(0,3);
103 
104  P_0(1,0)=P_init(1,0);
105  P_0(1,1)=P_init(1,1);
106  P_0(1,2)=P_init(1,2);
107  P_0(1,3)=P_init(1,3);
108 
109  P_0(2,0)=P_init(2,0);
110  P_0(2,1)=P_init(2,1);
111  P_0(2,2)=P_init(2,2);
112  P_0(2,3)=P_init(2,3);
113 
114  P_0(3,0)=P_init(3,0);
115  P_0(3,1)=P_init(3,1);
116  P_0(3,2)=P_init(3,2);
117  P_0(3,3)=P_init(3,3);
118 
119 // cout<<"initing stuff"<<endl;
120 // cout<<"xinit: "<<x_init<<endl;
121 // cout<<"pinit: "<<P_init<<endl;
122 
123  init(x_0,P_0);
124 
125 // cout<<"check if init is ok"<<endl;
126 // cout<<"x: "<<x<<endl;
127 // cout<<"P: "<<calculateP()<<endl;
128 
129 }
130 
132 {
133  dt=1./50.;
134 }
135 
137 {
138  A(0,0)=1.0;
139  A(0,1)=0.0;
140 // A(0,2)=dt;
141  A(0,3)=0.0;
142 
143  A(1,0)=0.0;
144  A(1,1)=1.0;
145  A(1,2)=0.0;
146 // A(1,3)=dt;
147 
148  A(2,0)=0.0;
149  A(2,1)=0.0;
150  A(2,2)=1.0;
151  A(2,3)=0.0;
152 
153  A(3,0)=0.0;
154  A(3,1)=0.0;
155  A(3,2)=0.0;
156  A(3,3)=1.0;
157 }
158 
160 {
161  A(0,2)=dt;
162  A(1,3)=dt;
163 }
164 
166 {
167  H(0,0) = 1.0;
168  H(0,1) = 0.0;
169  H(1,1) = 1.0;
170  H(1,0) = 0.0;
171 }
172 
174 {
175  SetIdentity(W,4,1.0);
176 }
177 
179 {
180  SetIdentity(V,2,1.0);
181 }
182 
184 {
185  //Innovation dependent error
186 // inovation_error //type Vetor2d
187 
188  Matrix2d eigval;
189 
190  double inovation_factor = 3.0;
191  double distortion_y = 0.1;
192 
193  double x_min = 0.2;
194  double y_min = 0.2;
195 
196  double x_size = inovation_error.norm()*inovation_factor;
197  double y_size = inovation_error.norm()*inovation_factor*distortion_y;
198 
199  double dir = atan2(inovation_error(1),inovation_error(0));
200 
201 
202  if(miss_associations>0)
203  {
204  x_size+=5;
205  y_size+=5;
206  }
207 
208  //Velocity dependent error
209 // Vector2d vel;
210 //
211 // vel(0)=x(2);//x
212 // vel(1)=x(3);//y
213 //
214 //
215 // Matrix2d eigval;
216 //
217 // double velocity_factor = 1.0;
218 // double distortion_y = 0.1;
219 //
220 // double x_min = 0.2;
221 // double y_min = 0.2;
222 //
223 // double x_size = vel.norm()*velocity_factor;
224 // double y_size = vel.norm()*velocity_factor*distortion_y;
225 
226 // double dir = atan2(vel(1),vel(0));
227 
228  if(y_size<y_min)
229  y_size = y_min;
230 
231  if(x_size<x_min)
232  x_size = x_min;
233 
234 
235  eigval << x_size, 0,
236  0, y_size;
237 
238  Matrix2d rot;
239  rot << cos(dir), -sin(dir),
240  sin(dir), cos(dir);
241 
242  Matrix2d cov = rot*eigval*rot.inverse();
243 
244 // cout<<"making R"<<endl;
245 // R(0,0) = 0.1;
246 // R(0,1) = 0.0;
247 
248 // R(1,0) = 0.0;
249 // R(1,1) = 0.1;
250 //
251 // if(miss_association)
252 // {
253 // R(0,0) = cov(0,0)*100;
254 // R(0,1) = cov(0,1)*100;
255 
256 // R(1,0) = cov(1,0)*100;
257 // R(1,1) = cov(1,1)*100;
258 // cout<<"coaissa"<<endl;
259 // }else
260 // {
261 
262 // cout<<"vel: "<<vel.norm()<<endl;
263 
264 // if(magnitude>0.1)
265 // {
266 // R(0,0) = 10;
267 // R(0,1) = 0;
268 //
269 // R(1,0) = 0;
270 // R(1,1) = 0.1;
271 // }
272 // else
273 // {
274 // R(0,0) = 1;
275 // R(0,1) = 0;
276 //
277 // R(1,0) = 0;
278 // R(1,1) = 1;
279 // }
280  R(0,0) = cov(0,0);
281  R(0,1) = cov(0,1);
282 //
283  R(1,0) = cov(1,0);
284  R(1,1) = cov(1,1);
285 // }
286 
287 // cout<<"R:"<<R<<endl;
288 }
289 
291 {
292  SetZero(Q,4);
293 // double sigma=10.0;
294 // double sigma=5.0;
295 // double sigma=2.0;
296 // double sigma=1.0;
297 // double sigma=0.5;
298 
299  double sigma;
300 
301  if(life_time<10)
302  sigma=150;
303  else if(life_time>12)
304  sigma=100;
305  else if(life_time<15)
306  sigma=50;
307  else
308  sigma=10;
309 
310 
311  double factor=pow(sigma,2)*dt/6.;
312 
313  Q(0,0)=factor*2*pow(dt,2);//x x
314  Q(1,1)=factor*2*pow(dt,2);//y y
315 
316  Q(0,2)=factor*3*pow(dt,1);//x vx
317  Q(2,1)=factor*3*pow(dt,1);//vx v
318 
319  Q(1,3)=factor*3*pow(dt,1);//y vy
320  Q(3,1)=factor*3*pow(dt,1);//vy y
321 
322  Q(2,2)=factor*6;//vx vx
323  Q(3,3)=factor*6;//vy vy
324 }
325 
327 {
328  Vector x_(x.size());
329 
330  x_(0) = x(0) + x(2)*dt;//X
331  x_(1) = x(1) + x(3)*dt;//Y
332  x_(2) = x(2);//Vx
333  x_(3) = x(3);//Vy
334 
335  x.swap(x_);
336 }
337 
339 {
340  z(0)=x(0); //X
341  z(1)=x(1); //Y
342 }
void SetIdentity(kMatrix &M, int size, double value=1)
Set a matrix to identity.
Definition: mht_types.cpp:37
void makeBaseV()
Make measurement noise sensitivity matrix.
Definition: mht_types.cpp:178
void makeProcess()
Make process, model iteration.
Definition: mht_types.cpp:326
void makeBaseH()
Make measurement sensitivity matrix.
Definition: mht_types.cpp:165
void makeR()
Make measurement noise covariance matrix.
Definition: mht_types.cpp:183
void SetZero(kMatrix &M, int size)
Set a matrix to zero.
Definition: mht_types.cpp:47
void makeBaseA()
Make the base constant process Jacobian matrix.
Definition: mht_types.cpp:136
double dt
Time interval between measurements.
Definition: mht_types.h:148
void InitFilter(Vector4d &x_init)
Init filter.
Definition: mht_types.cpp:54
void makeBaseW()
Make process noise sensitivity matrix.
Definition: mht_types.cpp:173
Definition: matrix.h:32
Time lt
Time of the last call to the makeCommonProcess function.
Definition: mht_types.h:150
long association_errors
Definition: mht_types.cpp:35
MHT generic types source code, NOT USED, DEPRECATED.
void makeQ()
Make process noise covariance matrix.
Definition: mht_types.cpp:290
EKFilter< double, 0, false, false, false >::Vector Vector
EKfilter vector type.
void makeA()
Make the process Jacobian matrix.
Definition: mht_types.cpp:159
void makeCommonProcess()
This function is called before all other make something functions.
Definition: mht_types.cpp:131
void makeMeasure()
Make measurement, used when measurement is not possible (i'm not using it now)
Definition: mht_types.cpp:338


mtt
Author(s): Jorge Almeida
autogenerated on Mon Mar 2 2015 01:32:18