@@ -48,7 +48,7 @@ template <typename Type, auto Size> using vector = Eigen::Vector<Type, Size>;
48
48
filter.x (trigger_strength, thermal_radius, thermal_position_x,
49
49
thermal_position_y);
50
50
51
- const float strength_covariance{0.0049 };
51
+ const float strength_covariance{0 .0049f };
52
52
const float radius_covariance{400 };
53
53
const float position_covariance{400 };
54
54
filter.p (kalman::estimate_uncertainty{{strength_covariance, 0 , 0 , 0 },
@@ -114,107 +114,107 @@ template <typename Type, auto Size> using vector = Eigen::Vector<Type, Size>;
114
114
115
115
// A hundred randomly generated data point.
116
116
constexpr data measured[]{
117
- {0.0756891 , 0.749786 , 0.878827 , 0.806808 , 0.155487 },
118
- {0.506366 , 0.261469 , 0.886986 , 0.332883 , 0.434406 },
119
- {0.249769 , 0.242154 , 0.616454 , 0.672545 , 0.24927 },
120
- {0.358587 , 0.556206 , 0.909985 , 0.370336 , 0.553264 },
121
- {0.370579 , 0.368003 , 0.491917 , 0.635429 , 0.73594 },
122
- {0.82946 , 0.0221123 , 0.461047 , 0.940697 , 0.987409 },
123
- {0.462132 , 0.708865 , 0.941915 , 0.122432 , 0.911597 },
124
- {0.888334 , 0.542419 , 0.773781 , 0.116075 , 0.917592 },
125
- {0.229376 , 0.174244 , 0.972009 , 0.509611 , 0.37637 },
126
- {0.887738 , 0.707866 , 0.90959 , 0.430274 , 0.242523 },
127
- {0.40713 , 0.0696747 , 0.456659 , 0.979656 , 0.11167 },
128
- {0.77115 , 0.183994 , 0.944587 , 0.467626 , 0.0219546 },
129
- {0.137442 , 0.316077 , 0.660742 , 0.828009 , 0.852228 },
130
- {0.128113 , 0.0757587 , 0.742959 , 0.360531 , 0.3932 },
131
- {0.161107 , 0.709262 , 0.690847 , 0.161165 , 0.237205 },
132
- {0.664184 , 0.658516 , 0.972067 , 0.465567 , 0.807259 },
133
- {0.669789 , 0.236436 , 0.341701 , 0.430546 , 0.229097 },
134
- {0.159471 , 0.122824 , 0.975034 , 0.833685 , 0.78011 },
135
- {0.284848 , 0.917524 , 0.358084 , 0.82927 , 0.0983398 },
136
- {0.209027 , 0.573124 , 0.428336 , 0.106116 , 0.17974 },
137
- {0.861987 , 0.110099 , 0.0994602 , 0.208052 , 0.0545667 },
138
- {0.483002 , 0.707016 , 0.189368 , 0.0626376 , 0.992816 },
139
- {0.588928 , 0.644143 , 0.763512 , 0.444366 , 0.251652 },
140
- {0.419946 , 0.338175 , 0.286543 , 0.97232 , 0.908061 },
141
- {0.0625373 , 0.855109 , 0.763831 , 0.622934 , 0.364608 },
142
- {0.55833 , 0.505803 , 0.600797 , 0.342724 , 0.735087 },
143
- {0.664873 , 0.224638 , 0.385409 , 0.892807 , 0.695 },
144
- {0.255295 , 0.0264766 , 0.229274 , 0.723291 , 0.552242 },
145
- {0.412129 , 0.856404 , 0.395075 , 0.261842 , 0.947885 },
146
- {0.468212 , 0.849367 , 0.00615251 , 0.842904 , 0.700869 },
147
- {0.311582 , 0.293401 , 0.299637 , 0.567025 , 0.659598 },
148
- {0.695464 , 0.941376 , 0.21219 , 0.27813 , 0.289406 },
149
- {0.000397467 , 0.301337 , 0.71608 , 0.296278 , 0.718923 },
150
- {0.36314 , 0.263077 , 0.193163 , 0.295399 , 0.0523569 },
151
- {0.128381 , 0.572157 , 0.971297 , 0.516492 , 0.921166 },
152
- {0.596215 , 0.909239 , 0.133898 , 0.506903 , 0.0335569 },
153
- {0.444556 , 0.997721 , 0.348369 , 0.644847 , 0.80885 },
154
- {0.891465 , 0.0797467 , 0.85753 , 0.369457 , 0.418543 },
155
- {0.861948 , 0.520583 , 0.900797 , 0.153884 , 0.080031 },
156
- {0.169696 , 0.981169 , 0.406729 , 0.292696 , 0.831505 },
157
- {0.172591 , 0.349291 , 0.782213 , 0.534652 , 0.214628 },
158
- {0.875081 , 0.746097 , 0.0806311 , 0.15685 , 0.357471 },
159
- {0.519389 , 0.007303 , 0.18117 , 0.370993 , 0.427305 },
160
- {0.961372 , 0.218945 , 0.486608 , 0.618755 , 0.168813 },
161
- {0.537862 , 0.451312 , 0.384422 , 0.540216 , 0.525636 },
162
- {0.494387 , 0.162124 , 0.0136825 , 0.127037 , 0.803511 },
163
- {0.409087 , 0.991167 , 0.276877 , 0.188698 , 0.155701 },
164
- {0.851474 , 0.54778 , 0.133586 , 0.37391 , 0.137362 },
165
- {0.0148137 , 0.97396 , 0.945259 , 0.297432 , 0.260494 },
166
- {0.906864 , 0.13484 , 0.214258 , 0.924681 , 0.618572 },
167
- {0.141742 , 0.563986 , 0.502602 , 0.416297 , 0.97038 },
168
- {0.698555 , 0.406929 , 0.558199 , 0.875364 , 0.736008 },
169
- {0.175105 , 0.270328 , 0.332957 , 0.145101 , 0.765857 },
170
- {0.68083 , 0.125673 , 0.922594 , 0.831683 , 0.457214 },
171
- {0.520728 , 0.26214 , 0.458674 , 0.306454 , 0.783164 },
172
- {0.780442 , 0.472245 , 0.125185 , 0.460146 , 0.0847598 },
173
- {0.360083 , 0.0686402 , 0.328997 , 0.799852 , 0.818809 },
174
- {0.71546 , 0.717884 , 0.253842 , 0.812915 , 0.0141433 },
175
- {0.441185 , 0.171204 , 0.0432966 , 0.739241 , 0.448679 },
176
- {0.399117 , 0.148854 , 0.743042 , 0.0230124 , 0.378786 },
177
- {0.841239 , 0.292533 , 0.391296 , 0.734326 , 0.0597166 },
178
- {0.350847 , 0.519149 , 0.808508 , 0.113644 , 0.673261 },
179
- {0.229909 , 0.814871 , 0.118688 , 0.612729 , 0.354682 },
180
- {0.734755 , 0.675693 , 0.646155 , 0.0296504 , 0.405621 },
181
- {0.121731 , 0.231111 , 0.47879 , 0.733299 , 0.270893 },
182
- {0.732981 , 0.813999 , 0.597652 , 0.455436 , 0.691262 },
183
- {0.10297 , 0.534613 , 0.553605 , 0.777385 , 0.553588 },
184
- {0.441429 , 0.974205 , 0.120671 , 0.279931 , 0.624484 },
185
- {0.531836 , 0.697762 , 0.274009 , 0.827927 , 0.741129 },
186
- {0.745307 , 0.085542 , 0.473629 , 0.286912 , 0.175756 },
187
- {0.758466 , 0.268705 , 0.108006 , 0.291002 , 0.559732 },
188
- {0.632262 , 0.733193 , 0.919653 , 0.165692 , 0.84716 },
189
- {0.0107621 , 0.694084 , 0.35781 , 0.793076 , 0.0818898 },
190
- {0.17388 , 0.333606 , 0.867638 , 0.969285 , 0.887633 },
191
- {0.255376 , 0.180532 , 0.737631 , 0.869954 , 0.875926 },
192
- {0.525821 , 0.882517 , 0.224126 , 0.906093 , 0.557676 },
193
- {0.516693 , 0.986614 , 0.644313 , 0.00903489 , 0.207868 },
194
- {0.00175451 , 0.49772 , 0.436713 , 0.0418148 , 0.63547 },
195
- {0.559954 , 0.192099 , 0.0787102 , 0.976933 , 0.552542 },
196
- {0.983202 , 0.165426 , 0.136735 , 0.467933 , 0.626612 },
197
- {0.520497 , 0.593702 , 0.0155549 , 0.791301 , 0.635127 },
198
- {0.934924 , 0.0663795 , 0.513404 , 0.791586 , 0.68594 },
199
- {0.977299 , 0.682359 , 0.0689664 , 0.769369 , 0.169862 },
200
- {0.681586 , 0.900795 , 0.312534 , 0.854568 , 0.113097 },
201
- {0.0783791 , 0.340692 , 0.23686 , 0.5932 , 0.38193 },
202
- {0.430041 , 0.401364 , 0.88266 , 0.226286 , 0.514185 },
203
- {0.422123 , 0.713778 , 0.813105 , 0.960577 , 0.794308 },
204
- {0.0531423 , 0.930818 , 0.913336 , 0.382305 , 0.372521 },
205
- {0.91698 , 0.128078 , 0.901849 , 0.0860355 , 0.432365 },
206
- {0.749259 , 0.198112 , 0.538301 , 0.739992 , 0.909026 },
207
- {0.903781 , 0.206122 , 0.743227 , 0.700662 , 0.784729 },
208
- {0.914658 , 0.625943 , 0.697374 , 0.333459 , 0.213769 },
209
- {0.313091 , 0.0485961 , 0.625018 , 0.916347 , 0.363119 },
210
- {0.455916 , 0.982769 , 0.245987 , 0.555492 , 0.938798 },
211
- {0.0737146 , 0.324519 , 0.325405 , 0.677491 , 0.148078 },
212
- {0.918677 , 0.537612 , 0.917458 , 0.611973 , 0.965844 },
213
- {0.832977 , 0.466222 , 0.528761 , 0.348765 , 0.472975 },
214
- {0.784042 , 0.866144 , 0.00524178 , 0.217837 , 0.145246 },
215
- {0.308576 , 0.993283 , 0.0244056 , 0.543786 , 0.575841 },
216
- {0.285113 , 0.12198 , 0.74075 , 0.834888 , 0.561457 },
217
- {0.635992 , 0.590228 , 0.629378 , 0.112457 , 0.78253 }};
117
+ {0 .0756891f , 0 .749786f , 0 .878827f , 0 .806808f , 0 .155487f },
118
+ {0 .506366f , 0 .261469f , 0 .886986f , 0 .332883f , 0 .434406f },
119
+ {0 .249769f , 0 .242154f , 0 .616454f , 0 .672545f , 0 .24927f },
120
+ {0 .358587f , 0 .556206f , 0 .909985f , 0 .370336f , 0 .553264f },
121
+ {0 .370579f , 0 .368003f , 0 .491917f , 0 .635429f , 0 .73594f },
122
+ {0 .82946f , 0 .0221123f , 0 .461047f , 0 .940697f , 0 .987409f },
123
+ {0 .462132f , 0 .708865f , 0 .941915f , 0 .122432f , 0 .911597f },
124
+ {0 .888334f , 0 .542419f , 0 .773781f , 0 .116075f , 0 .917592f },
125
+ {0 .229376f , 0 .174244f , 0 .972009f , 0 .509611f , 0 .37637f },
126
+ {0 .887738f , 0 .707866f , 0 .90959f , 0 .430274f , 0 .242523f },
127
+ {0 .40713f , 0 .0696747f , 0 .456659f , 0 .979656f , 0 .11167f },
128
+ {0 .77115f , 0 .183994f , 0 .944587f , 0 .467626f , 0 .0219546f },
129
+ {0 .137442f , 0 .316077f , 0 .660742f , 0 .828009f , 0 .852228f },
130
+ {0 .128113f , 0 .0757587f , 0 .742959f , 0 .360531f , 0 .3932f },
131
+ {0 .161107f , 0 .709262f , 0 .690847f , 0 .161165f , 0 .237205f },
132
+ {0 .664184f , 0 .658516f , 0 .972067f , 0 .465567f , 0 .807259f },
133
+ {0 .669789f , 0 .236436f , 0 .341701f , 0 .430546f , 0 .229097f },
134
+ {0 .159471f , 0 .122824f , 0 .975034f , 0 .833685f , 0 .78011f },
135
+ {0 .284848f , 0 .917524f , 0 .358084f , 0 .82927f , 0 .0983398f },
136
+ {0 .209027f , 0 .573124f , 0 .428336f , 0 .106116f , 0 .17974f },
137
+ {0 .861987f , 0 .110099f , 0 .0994602f , 0 .208052f , 0 .0545667f },
138
+ {0 .483002f , 0 .707016f , 0 .189368f , 0 .0626376f , 0 .992816f },
139
+ {0 .588928f , 0 .644143f , 0 .763512f , 0 .444366f , 0 .251652f },
140
+ {0 .419946f , 0 .338175f , 0 .286543f , 0 .97232f , 0 .908061f },
141
+ {0 .0625373f , 0 .855109f , 0 .763831f , 0 .622934f , 0 .364608f },
142
+ {0 .55833f , 0 .505803f , 0 .600797f , 0 .342724f , 0 .735087f },
143
+ {0 .664873f , 0 .224638f , 0 .385409f , 0 .892807f , 0 .695f },
144
+ {0 .255295f , 0 .0264766f , 0 .229274f , 0 .723291f , 0 .552242f },
145
+ {0 .412129f , 0 .856404f , 0 .395075f , 0 .261842f , 0 .947885f },
146
+ {0 .468212f , 0 .849367f , 0 .00615251f , 0 .842904f , 0 .700869f },
147
+ {0 .311582f , 0 .293401f , 0 .299637f , 0 .567025f , 0 .659598f },
148
+ {0 .695464f , 0 .941376f , 0 .21219f , 0 .27813f , 0 .289406f },
149
+ {0 .000397467f , 0 .301337f , 0 .71608f , 0 .296278f , 0 .718923f },
150
+ {0 .36314f , 0 .263077f , 0 .193163f , 0 .295399f , 0 .0523569f },
151
+ {0 .128381f , 0 .572157f , 0 .971297f , 0 .516492f , 0 .921166f },
152
+ {0 .596215f , 0 .909239f , 0 .133898f , 0 .506903f , 0 .0335569f },
153
+ {0 .444556f , 0 .997721f , 0 .348369f , 0 .644847f , 0 .80885f },
154
+ {0 .891465f , 0 .0797467f , 0 .85753f , 0 .369457f , 0 .418543f },
155
+ {0 .861948f , 0 .520583f , 0 .900797f , 0 .153884f , 0 .080031f },
156
+ {0 .169696f , 0 .981169f , 0 .406729f , 0 .292696f , 0 .831505f },
157
+ {0 .172591f , 0 .349291f , 0 .782213f , 0 .534652f , 0 .214628f },
158
+ {0 .875081f , 0 .746097f , 0 .0806311f , 0 .15685f , 0 .357471f },
159
+ {0 .519389f , 0 .007303f , 0 .18117f , 0 .370993f , 0 .427305f },
160
+ {0 .961372f , 0 .218945f , 0 .486608f , 0 .618755f , 0 .168813f },
161
+ {0 .537862f , 0 .451312f , 0 .384422f , 0 .540216f , 0 .525636f },
162
+ {0 .494387f , 0 .162124f , 0 .0136825f , 0 .127037f , 0 .803511f },
163
+ {0 .409087f , 0 .991167f , 0 .276877f , 0 .188698f , 0 .155701f },
164
+ {0 .851474f , 0 .54778f , 0 .133586f , 0 .37391f , 0 .137362f },
165
+ {0 .0148137f , 0 .97396f , 0 .945259f , 0 .297432f , 0 .260494f },
166
+ {0 .906864f , 0 .13484f , 0 .214258f , 0 .924681f , 0 .618572f },
167
+ {0 .141742f , 0 .563986f , 0 .502602f , 0 .416297f , 0 .97038f },
168
+ {0 .698555f , 0 .406929f , 0 .558199f , 0 .875364f , 0 .736008f },
169
+ {0 .175105f , 0 .270328f , 0 .332957f , 0 .145101f , 0 .765857f },
170
+ {0 .68083f , 0 .125673f , 0 .922594f , 0 .831683f , 0 .457214f },
171
+ {0 .520728f , 0 .26214f , 0 .458674f , 0 .306454f , 0 .783164f },
172
+ {0 .780442f , 0 .472245f , 0 .125185f , 0 .460146f , 0 .0847598f },
173
+ {0 .360083f , 0 .0686402f , 0 .328997f , 0 .799852f , 0 .818809f },
174
+ {0 .71546f , 0 .717884f , 0 .253842f , 0 .812915f , 0 .0141433f },
175
+ {0 .441185f , 0 .171204f , 0 .0432966f , 0 .739241f , 0 .448679f },
176
+ {0 .399117f , 0 .148854f , 0 .743042f , 0 .0230124f , 0 .378786f },
177
+ {0 .841239f , 0 .292533f , 0 .391296f , 0 .734326f , 0 .0597166f },
178
+ {0 .350847f , 0 .519149f , 0 .808508f , 0 .113644f , 0 .673261f },
179
+ {0 .229909f , 0 .814871f , 0 .118688f , 0 .612729f , 0 .354682f },
180
+ {0 .734755f , 0 .675693f , 0 .646155f , 0 .0296504f , 0 .405621f },
181
+ {0 .121731f , 0 .231111f , 0 .47879f , 0 .733299f , 0 .270893f },
182
+ {0 .732981f , 0 .813999f , 0 .597652f , 0 .455436f , 0 .691262f },
183
+ {0 .10297f , 0 .534613f , 0 .553605f , 0 .777385f , 0 .553588f },
184
+ {0 .441429f , 0 .974205f , 0 .120671f , 0 .279931f , 0 .624484f },
185
+ {0 .531836f , 0 .697762f , 0 .274009f , 0 .827927f , 0 .741129f },
186
+ {0 .745307f , 0 .085542f , 0 .473629f , 0 .286912f , 0 .175756f },
187
+ {0 .758466f , 0 .268705f , 0 .108006f , 0 .291002f , 0 .559732f },
188
+ {0 .632262f , 0 .733193f , 0 .919653f , 0 .165692f , 0 .84716f },
189
+ {0 .0107621f , 0 .694084f , 0 .35781f , 0 .793076f , 0 .0818898f },
190
+ {0 .17388f , 0 .333606f , 0 .867638f , 0 .969285f , 0 .887633f },
191
+ {0 .255376f , 0 .180532f , 0 .737631f , 0 .869954f , 0 .875926f },
192
+ {0 .525821f , 0 .882517f , 0 .224126f , 0 .906093f , 0 .557676f },
193
+ {0 .516693f , 0 .986614f , 0 .644313f , 0 .00903489f , 0 .207868f },
194
+ {0 .00175451f , 0 .49772f , 0 .436713f , 0 .0418148f , 0 .63547f },
195
+ {0 .559954f , 0 .192099f , 0 .0787102f , 0 .976933f , 0 .552542f },
196
+ {0 .983202f , 0 .165426f , 0 .136735f , 0 .467933f , 0 .626612f },
197
+ {0 .520497f , 0 .593702f , 0 .0155549f , 0 .791301f , 0 .635127f },
198
+ {0 .934924f , 0 .0663795f , 0 .513404f , 0 .791586f , 0 .68594f },
199
+ {0 .977299f , 0 .682359f , 0 .0689664f , 0 .769369f , 0 .169862f },
200
+ {0 .681586f , 0 .900795f , 0 .312534f , 0 .854568f , 0 .113097f },
201
+ {0 .0783791f , 0 .340692f , 0 .23686f , 0 .5932f , 0 .38193f },
202
+ {0 .430041f , 0 .401364f , 0 .88266f , 0 .226286f , 0 .514185f },
203
+ {0 .422123f , 0 .713778f , 0 .813105f , 0 .960577f , 0 .794308f },
204
+ {0 .0531423f , 0 .930818f , 0 .913336f , 0 .382305f , 0 .372521f },
205
+ {0 .91698f , 0 .128078f , 0 .901849f , 0 .0860355f , 0 .432365f },
206
+ {0 .749259f , 0 .198112f , 0 .538301f , 0 .739992f , 0 .909026f },
207
+ {0 .903781f , 0 .206122f , 0 .743227f , 0 .700662f , 0 .784729f },
208
+ {0 .914658f , 0 .625943f , 0 .697374f , 0 .333459f , 0 .213769f },
209
+ {0 .313091f , 0 .0485961f , 0 .625018f , 0 .916347f , 0 .363119f },
210
+ {0 .455916f , 0 .982769f , 0 .245987f , 0 .555492f , 0 .938798f },
211
+ {0 .0737146f , 0 .324519f , 0 .325405f , 0 .677491f , 0 .148078f },
212
+ {0 .918677f , 0 .537612f , 0 .917458f , 0 .611973f , 0 .965844f },
213
+ {0 .832977f , 0 .466222f , 0 .528761f , 0 .348765f , 0 .472975f },
214
+ {0 .784042f , 0 .866144f , 0 .00524178f , 0 .217837f , 0 .145246f },
215
+ {0 .308576f , 0 .993283f , 0 .0244056f , 0 .543786f , 0 .575841f },
216
+ {0 .285113f , 0 .12198f , 0 .74075f , 0 .834888f , 0 .561457f },
217
+ {0 .635992f , 0 .590228f , 0 .629378f , 0 .112457f , 0 .78253f }};
218
218
219
219
for (const auto &output : measured) {
220
220
filter.predict (output.drift_x , output.drift_y );
0 commit comments