18 #include <gtest/gtest.h>
25 #define CHECK_AND_RET_ERROR(_COND_, _MSG_) EXPECT_FALSE(_COND_) << _MSG_;
27 TEST(Matrices, HCHt_3x2_2x2_2x3)
29 const double dat_H[] = {0.2, -0.3, 0.1, 0.9, -0.07, 1.2};
32 const double dat_C[] = {0.8, -0.1, -0.1, 0.8};
35 const double dat_R[] = {0.11600, -0.21500, -0.32530, -0.21500, 0.63800,
36 0.85270, -0.32530, 0.85270, 1.17272};
40 H.multiply_HCHt(C,
R);
42 EXPECT_NEAR((R_REAL -
R).array().abs().
sum(), 0, 1e-4);
45 TEST(Matrices, HCHt_scalar_1x2_2x2_2x1)
47 const double dat_H[] = {0.2, -0.3};
50 const double dat_C[] = {0.8, -0.1, -0.1, 0.8};
53 const double r = H.multiply_HCHt_scalar(C);
55 const double r2 = (H * C * H.transpose()).eval()(0, 0);
57 fabs(
r - r2) > 1e-4,
"Error in HCHt_scalar: 1x2 * 2x2 * 2x1")
60 TEST(Matrices, det_2x2_dyn)
62 const double dat_A[] = {0.8, -0.3, -0.7, 0.1};
65 const double d = A.det();
66 const double d_R = -0.13;
71 TEST(Matrices, det_2x2_fix)
73 const double dat_A[] = {0.8, -0.3, -0.7, 0.1};
76 const double d = A.det();
77 const double d_R = -0.13;
82 TEST(Matrices, det_3x3_dyn)
84 const double dat_A[] = {-3.3304e-01, -2.0585e-01, 6.2026e-05,
85 1.4631e+00, 6.0985e-01, 2.3746e+00,
86 -3.6451e-01, 4.8169e-01, -8.4419e-01};
88 const double d = A.det();
89 const double d_R = 0.476380435871666;
94 TEST(Matrices, det_3x3_fix)
96 const double dat_A[] = {-3.3304e-01, -2.0585e-01, 6.2026e-05,
97 1.4631e+00, 6.0985e-01, 2.3746e+00,
98 -3.6451e-01, 4.8169e-01, -8.4419e-01};
100 const double d = A.det();
101 const double d_R = 0.476380435871666;
107 const double dat_A[] = {0.773931, -0.336130, 1.131764, 0.385890,
108 1.374906, -0.540629, -0.952902, 0.659769,
109 -0.387254, -1.557355, 0.139683, -2.056635,
110 -0.750078, -0.653811, 0.872027, 0.217554};
112 const double d = A.det();
113 const double d_R = -6.29527837425056;
119 const double dat_A[] = {0.773931, -0.336130, 1.131764, 0.385890,
120 1.374906, -0.540629, -0.952902, 0.659769,
121 -0.387254, -1.557355, 0.139683, -2.056635,
122 -0.750078, -0.653811, 0.872027, 0.217554};
124 const double d = A.det();
125 const double d_R = -6.29527837425056;
131 const double dat_A[] = {
132 1.2305462976, -0.2944257811, 0.8176140437, -0.0487601371,
133 0.4418235581, -0.0088466980, -1.4100223408, -0.6219629815,
134 1.1089237266, -0.6450262619, -2.0862614547, 0.2699762709,
135 -0.0705918517, 1.1763963161, -0.3461819597, -1.3013222580,
136 -0.3310621595, -0.2595069675, -0.5188213591, 1.2261476224,
137 -1.1334297957, 2.1452881319, 1.7856021357, 0.5406722888,
138 0.5497545623, 0.4282217402, -1.6175210256, -0.3522824764,
139 0.2773929603, 0.8507134453, 0.4046854117, -2.1638696195,
140 1.0044939778, 0.9755939720, 0.9640788301, 0.5641138097,
141 0.7382236207, -0.4422212587, 0.8507041571, 1.3764399072,
142 0.3446492224, 1.1681336612, -1.3440052449, 1.0120691406,
143 -0.0430604384, 0.4823901171, 0.0881769800, 0.3984805283,
144 -1.9988153178, 0.9509748328, 0.3202853059, 1.9688559025,
145 0.4020581289, -1.5558616735, -0.8753527614, 0.1207830427,
146 0.0457715031, -0.1557123759, -0.3161307172, -0.0759276933,
147 -0.0417386037, 1.2079564736, -2.5839030155, -0.7648863647,
148 1.1541464803, 0.2127569446, -1.4882083860, -0.7630836781,
149 0.8550884427, -0.8440402465, -0.4903597050, -0.1457982930,
150 0.5893448560, -0.2353784687, 0.3474655757, 2.5874616045,
151 0.6608448038, -1.0105315509, -1.5276853710, -0.1400026815,
152 -1.7630264416, 2.4048579514, -0.3111046623, 0.7463774799,
153 -0.2800404492, -1.4175124130, -0.5708536580, -1.2085107661,
154 0.8169107561, -1.1659481510, -0.1406355512, 2.3507381980,
155 2.6346742737, -1.1028788167, -0.0533115044, 0.3752684649,
156 -1.3799576309, -0.7274190037, 1.1188847602, -0.6624231096};
158 const double d = A.det();
159 const double d_R = 330.498518199239;
165 const double dat_A[] = {
166 1.2305462976, -0.2944257811, 0.8176140437, -0.0487601371,
167 0.4418235581, -0.0088466980, -1.4100223408, -0.6219629815,
168 1.1089237266, -0.6450262619, -2.0862614547, 0.2699762709,
169 -0.0705918517, 1.1763963161, -0.3461819597, -1.3013222580,
170 -0.3310621595, -0.2595069675, -0.5188213591, 1.2261476224,
171 -1.1334297957, 2.1452881319, 1.7856021357, 0.5406722888,
172 0.5497545623, 0.4282217402, -1.6175210256, -0.3522824764,
173 0.2773929603, 0.8507134453, 0.4046854117, -2.1638696195,
174 1.0044939778, 0.9755939720, 0.9640788301, 0.5641138097,
175 0.7382236207, -0.4422212587, 0.8507041571, 1.3764399072,
176 0.3446492224, 1.1681336612, -1.3440052449, 1.0120691406,
177 -0.0430604384, 0.4823901171, 0.0881769800, 0.3984805283,
178 -1.9988153178, 0.9509748328, 0.3202853059, 1.9688559025,
179 0.4020581289, -1.5558616735, -0.8753527614, 0.1207830427,
180 0.0457715031, -0.1557123759, -0.3161307172, -0.0759276933,
181 -0.0417386037, 1.2079564736, -2.5839030155, -0.7648863647,
182 1.1541464803, 0.2127569446, -1.4882083860, -0.7630836781,
183 0.8550884427, -0.8440402465, -0.4903597050, -0.1457982930,
184 0.5893448560, -0.2353784687, 0.3474655757, 2.5874616045,
185 0.6608448038, -1.0105315509, -1.5276853710, -0.1400026815,
186 -1.7630264416, 2.4048579514, -0.3111046623, 0.7463774799,
187 -0.2800404492, -1.4175124130, -0.5708536580, -1.2085107661,
188 0.8169107561, -1.1659481510, -0.1406355512, 2.3507381980,
189 2.6346742737, -1.1028788167, -0.0533115044, 0.3752684649,
190 -1.3799576309, -0.7274190037, 1.1188847602, -0.6624231096};
192 const double d = A.det();
193 const double d_R = 330.498518199239;
199 const double dat_A[] = {1.0727710178, 0.6393375593, 0.6393375593,
204 const double dat_CHOL[] = {1.0357465992, 0.6172721781, 0.0000000000,
208 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 2x2 dyn")
213 const double dat_A[] = {1.0727710178, 0.6393375593, 0.6393375593,
218 const double dat_CHOL[] = {1.0357465992, 0.6172721781, 0.0000000000,
222 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 2x2 fix")
227 const double dat_A[] = {
228 0.515479426556448, 0.832723636299236, 0.249691538245735,
229 0.832723636299236, 1.401081397506934, 0.385539356127255,
230 0.249691538245735, 0.385539356127255, 0.128633962591437};
235 const double dat_CHOL[] = {
236 0.717968959326549, 1.159832365288224, 0.347774837619643,
237 0.000000000000000, 0.236368952988455, -0.075395504153773,
238 0.000000000000000, 0.000000000000000, 0.044745311077990};
241 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 3x3 dyn")
246 const double dat_A[] = {
247 0.515479426556448, 0.832723636299236, 0.249691538245735,
248 0.832723636299236, 1.401081397506934, 0.385539356127255,
249 0.249691538245735, 0.385539356127255, 0.128633962591437};
254 const double dat_CHOL[] = {
255 0.717968959326549, 1.159832365288224, 0.347774837619643,
256 0.000000000000000, 0.236368952988455, -0.075395504153773,
257 0.000000000000000, 0.000000000000000, 0.044745311077990};
260 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 3x3 fix")
265 const double dat_A[] = {
266 2.8955668335, 2.3041932983, 1.9002381085, 1.7993158652, 1.8456197228,
267 2.9632296740, 1.9368565578, 2.1988923358, 2.0547605617, 2.5655678993,
268 2.3041932983, 3.8406914364, 2.1811218706, 3.2312564555, 2.4736403918,
269 3.4703311380, 1.4874417483, 3.1073538218, 2.1353324397, 2.9541115932,
270 1.9002381085, 2.1811218706, 2.4942067597, 1.6851007198, 1.4585872052,
271 2.3015952197, 1.0955231591, 2.2979627790, 1.3918738834, 2.1854562572,
272 1.7993158652, 3.2312564555, 1.6851007198, 3.1226161015, 1.6779632687,
273 2.7195826381, 1.2397348013, 2.3757864319, 1.6291224768, 2.4463194915,
274 1.8456197228, 2.4736403918, 1.4585872052, 1.6779632687, 2.8123267839,
275 2.5860688816, 1.4131630919, 2.1914803135, 1.5542420639, 2.7170092067,
276 2.9632296740, 3.4703311380, 2.3015952197, 2.7195826381, 2.5860688816,
277 4.1669180394, 2.1145239023, 3.3214801332, 2.6694845663, 3.0742063088,
278 1.9368565578, 1.4874417483, 1.0955231591, 1.2397348013, 1.4131630919,
279 2.1145239023, 1.8928811570, 1.7097998455, 1.7205860530, 1.8710847505,
280 2.1988923358, 3.1073538218, 2.2979627790, 2.3757864319, 2.1914803135,
281 3.3214801332, 1.7097998455, 3.4592638415, 2.1518695071, 2.8907499694,
282 2.0547605617, 2.1353324397, 1.3918738834, 1.6291224768, 1.5542420639,
283 2.6694845663, 1.7205860530, 2.1518695071, 2.1110960664, 1.6731209980,
284 2.5655678993, 2.9541115932, 2.1854562572, 2.4463194915, 2.7170092067,
285 3.0742063088, 1.8710847505, 2.8907499694, 1.6731209980, 3.9093678727};
290 const double dat_CHOL[] = {
291 1.7016365163, 1.3541042851, 1.1167121124, 1.0574031810,
292 1.0846145491, 1.7413999087, 1.1382316607, 1.2922221137,
293 1.2075202560, 1.5077061845, 0.0000000000, 1.4167191047,
294 0.4722017314, 1.2701334167, 0.7093566960, 0.7851196867,
295 -0.0380051491, 0.9582353452, 0.3530862859, 0.6441080558,
296 0.0000000000, 0.0000000000, 1.0120209201, -0.0943393725,
297 -0.0865342379, -0.0136183214, -0.1557357390, 0.3976620401,
298 -0.1218419159, 0.1952860421, 0.0000000000, 0.0000000000,
299 0.0000000000, 0.6183654266, -0.6113744707, -0.1944977093,
300 0.1127886805, -0.2752173394, -0.1741275611, 0.0847171764,
301 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
302 0.8668818973, 0.0234194680, 0.3011475111, -0.0272963639,
303 -0.1417917925, 0.8000162775, 0.0000000000, 0.0000000000,
304 0.0000000000, 0.0000000000, 0.0000000000, 0.6924364129,
305 0.2527445784, 0.3919505633, 0.3715689962, -0.0817608778,
306 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
307 0.0000000000, 0.0000000000, 0.6358623279, 0.4364121485,
308 0.4859857603, -0.0313828244, 0.0000000000, 0.0000000000,
309 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
310 0.0000000000, 0.5408375843, -0.1995475524, 0.6258606925,
311 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
312 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
313 0.2213262214, -0.2367037013, 0.0000000000, 0.0000000000,
314 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
315 0.0000000000, 0.0000000000, 0.0000000000, 0.2838575216};
318 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 10x10 dyn")
323 const double dat_A[] = {
324 2.8955668335, 2.3041932983, 1.9002381085, 1.7993158652, 1.8456197228,
325 2.9632296740, 1.9368565578, 2.1988923358, 2.0547605617, 2.5655678993,
326 2.3041932983, 3.8406914364, 2.1811218706, 3.2312564555, 2.4736403918,
327 3.4703311380, 1.4874417483, 3.1073538218, 2.1353324397, 2.9541115932,
328 1.9002381085, 2.1811218706, 2.4942067597, 1.6851007198, 1.4585872052,
329 2.3015952197, 1.0955231591, 2.2979627790, 1.3918738834, 2.1854562572,
330 1.7993158652, 3.2312564555, 1.6851007198, 3.1226161015, 1.6779632687,
331 2.7195826381, 1.2397348013, 2.3757864319, 1.6291224768, 2.4463194915,
332 1.8456197228, 2.4736403918, 1.4585872052, 1.6779632687, 2.8123267839,
333 2.5860688816, 1.4131630919, 2.1914803135, 1.5542420639, 2.7170092067,
334 2.9632296740, 3.4703311380, 2.3015952197, 2.7195826381, 2.5860688816,
335 4.1669180394, 2.1145239023, 3.3214801332, 2.6694845663, 3.0742063088,
336 1.9368565578, 1.4874417483, 1.0955231591, 1.2397348013, 1.4131630919,
337 2.1145239023, 1.8928811570, 1.7097998455, 1.7205860530, 1.8710847505,
338 2.1988923358, 3.1073538218, 2.2979627790, 2.3757864319, 2.1914803135,
339 3.3214801332, 1.7097998455, 3.4592638415, 2.1518695071, 2.8907499694,
340 2.0547605617, 2.1353324397, 1.3918738834, 1.6291224768, 1.5542420639,
341 2.6694845663, 1.7205860530, 2.1518695071, 2.1110960664, 1.6731209980,
342 2.5655678993, 2.9541115932, 2.1854562572, 2.4463194915, 2.7170092067,
343 3.0742063088, 1.8710847505, 2.8907499694, 1.6731209980, 3.9093678727};
348 const double dat_CHOL[] = {
349 1.7016365163, 1.3541042851, 1.1167121124, 1.0574031810,
350 1.0846145491, 1.7413999087, 1.1382316607, 1.2922221137,
351 1.2075202560, 1.5077061845, 0.0000000000, 1.4167191047,
352 0.4722017314, 1.2701334167, 0.7093566960, 0.7851196867,
353 -0.0380051491, 0.9582353452, 0.3530862859, 0.6441080558,
354 0.0000000000, 0.0000000000, 1.0120209201, -0.0943393725,
355 -0.0865342379, -0.0136183214, -0.1557357390, 0.3976620401,
356 -0.1218419159, 0.1952860421, 0.0000000000, 0.0000000000,
357 0.0000000000, 0.6183654266, -0.6113744707, -0.1944977093,
358 0.1127886805, -0.2752173394, -0.1741275611, 0.0847171764,
359 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
360 0.8668818973, 0.0234194680, 0.3011475111, -0.0272963639,
361 -0.1417917925, 0.8000162775, 0.0000000000, 0.0000000000,
362 0.0000000000, 0.0000000000, 0.0000000000, 0.6924364129,
363 0.2527445784, 0.3919505633, 0.3715689962, -0.0817608778,
364 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
365 0.0000000000, 0.0000000000, 0.6358623279, 0.4364121485,
366 0.4859857603, -0.0313828244, 0.0000000000, 0.0000000000,
367 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
368 0.0000000000, 0.5408375843, -0.1995475524, 0.6258606925,
369 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
370 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
371 0.2213262214, -0.2367037013, 0.0000000000, 0.0000000000,
372 0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
373 0.0000000000, 0.0000000000, 0.0000000000, 0.2838575216};
376 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 10x10 fix")