Skip to content

Commit f29b862

Browse files
author
Tobias Haeckel
committed
added Changes according to PR
change for Time Sync from uint64_t to int64_t is still missing
1 parent 28650bd commit f29b862

File tree

3 files changed

+76
-106
lines changed

3 files changed

+76
-106
lines changed

‎lib/APPLineFollowerSensorFusion/src/App.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -466,7 +466,7 @@ void App::publishVehicleAndSensorSnapshot(const VehicleData& data)
466466

467467
void App::filterLocationData(const VehicleData& vehicleData, const SpaceShipRadarPose& ssrPose)
468468
{
469-
/* Local variables (all declared at top as requested). */
469+
/* Local variables. */
470470
uint32_t zumoTs32 = 0U;
471471
uint32_t zumoLocalMs32 = 0U;
472472
uint32_t ssrLocalMs32 = 0U;

‎lib/APPLineFollowerSensorFusion/src/EKF.cpp‎

Lines changed: 60 additions & 72 deletions
Original file line numberDiff line numberDiff line change
@@ -42,14 +42,14 @@ namespace
4242
constexpr float TWO_PI_MRAD = 2.0F * PI_MRAD;
4343

4444
/** Default process noise standard deviations. */
45-
constexpr float SIGMA_PX = 1.0F; /* [mm] */
46-
constexpr float SIGMA_PY = 1.0F; /* [mm] */
47-
constexpr float SIGMA_THETA_P = 5.0F; /* [mrad] */
48-
constexpr float SIGMA_V_P = 5.0F; /* [mm/s] */
49-
constexpr float SIGMA_OMEGA_P = 2.0F; /* [mrad/s] */
45+
constexpr float SIGMA_PX = 1.0F; /* [mm] */
46+
constexpr float SIGMA_PY = 1.0F; /* [mm] */
47+
constexpr float SIGMA_THETA_P = 5.0F; /* [mrad] */
48+
constexpr float SIGMA_V_P = 5.0F; /* [mm/s] */
49+
constexpr float SIGMA_OMEGA_P = 2.0F; /* [mrad/s] */
5050

5151
/** Camera noise. */
52-
constexpr float SIGMA_CAM_POS = 2.0F; /* [mm] */
52+
constexpr float SIGMA_CAM_POS = 2.0F; /* [mm] */
5353
constexpr float SIGMA_CAM_THETA = 5.0F; /* [mrad] */
5454
constexpr float SIGMA_CAM_V = 20.0F; /* [mm/s] */
5555

@@ -60,15 +60,15 @@ namespace
6060
constexpr float SIGMA_ODO_THETA = 5.0F; /* [mrad] */
6161

6262
/** IMU yaw noise. */
63-
constexpr float SIGMA_IMU_OMEGA = 1.0F; /* [mrad/s] */
63+
constexpr float SIGMA_IMU_OMEGA = 1.0F; /* [mrad/s] */
6464

6565
/** Default initial state. */
66-
constexpr float EKF_START_X_MM = 705.0F; /* [mm] */
67-
constexpr float EKF_START_Y_MM = 939.0F; /* [mm] */
68-
constexpr float EKF_START_THETA_MRAD = 0.0F; /* [mrad] */
69-
constexpr float EKF_START_V_MMS = 0.0F; /* [mm/s] */
66+
constexpr float EKF_START_X_MM = 705.0F; /* [mm] */
67+
constexpr float EKF_START_Y_MM = 939.0F; /* [mm] */
68+
constexpr float EKF_START_THETA_MRAD = 0.0F; /* [mrad] */
69+
constexpr float EKF_START_V_MMS = 0.0F; /* [mm/s] */
7070
constexpr float EKF_START_OMEGA_MRADS = 0.0F; /* [mrad/s] */
71-
}
71+
} // namespace
7272

7373
/******************************************************************************
7474
* Public Methods
@@ -82,25 +82,25 @@ ExtendedKalmanFilter5D::ExtendedKalmanFilter5D()
8282

8383
/* Process noise Q (diagonal). */
8484
m_Q.setZero();
85-
m_Q(0, 0) = SIGMA_PX * SIGMA_PX;
86-
m_Q(1, 1) = SIGMA_PY * SIGMA_PY;
85+
m_Q(0, 0) = SIGMA_PX * SIGMA_PX;
86+
m_Q(1, 1) = SIGMA_PY * SIGMA_PY;
8787
m_Q(2, 2) = SIGMA_THETA_P * SIGMA_THETA_P;
88-
m_Q(3, 3) = SIGMA_V_P * SIGMA_V_P;
88+
m_Q(3, 3) = SIGMA_V_P * SIGMA_V_P;
8989
m_Q(4, 4) = SIGMA_OMEGA_P * SIGMA_OMEGA_P;
9090

9191
/* Camera measurement noise R_cam. */
9292
m_R_cam.setZero();
93-
m_R_cam(0, 0) = SIGMA_CAM_POS * SIGMA_CAM_POS;
94-
m_R_cam(1, 1) = SIGMA_CAM_POS * SIGMA_CAM_POS;
93+
m_R_cam(0, 0) = SIGMA_CAM_POS * SIGMA_CAM_POS;
94+
m_R_cam(1, 1) = SIGMA_CAM_POS * SIGMA_CAM_POS;
9595
m_R_cam(2, 2) = SIGMA_CAM_THETA * SIGMA_CAM_THETA;
96-
m_R_cam(3, 3) = SIGMA_CAM_V * SIGMA_CAM_V;
97-
m_R_cam(4, 4) = SIGMA_CAM_V * SIGMA_CAM_V;
96+
m_R_cam(3, 3) = SIGMA_CAM_V * SIGMA_CAM_V;
97+
m_R_cam(4, 4) = SIGMA_CAM_V * SIGMA_CAM_V;
9898

9999
/* Odometry measurement noise R_odo. */
100100
m_R_odo.setZero();
101101
m_R_odo(0, 0) = SIGMA_ODO_POS_X * SIGMA_ODO_POS_X;
102102
m_R_odo(1, 1) = SIGMA_ODO_POS_Y * SIGMA_ODO_POS_Y;
103-
m_R_odo(2, 2) = SIGMA_ODO_V * SIGMA_ODO_V;
103+
m_R_odo(2, 2) = SIGMA_ODO_V * SIGMA_ODO_V;
104104
m_R_odo(3, 3) = SIGMA_ODO_THETA * SIGMA_ODO_THETA;
105105

106106
/* IMU yaw noise R_imu. */
@@ -129,8 +129,7 @@ void ExtendedKalmanFilter5D::init()
129129
void ExtendedKalmanFilter5D::predict(float accX_raw, float dt)
130130
{
131131
/* Convert raw accelerometer digits to physical acceleration [mm/s^2]. */
132-
const float a_x_mms =
133-
accX_raw * SensorConstants::ACCELEROMETER_SENSITIVITY_FACTOR;
132+
const float a_x_mms = accX_raw * SensorConstants::ACCELEROMETER_SENSITIVITY_FACTOR;
134133

135134
/* Nonlinear prediction. */
136135
StateVector x_pred = processModel(m_state, a_x_mms, dt);
@@ -152,8 +151,8 @@ void ExtendedKalmanFilter5D::predict(float accX_raw, float dt)
152151
void ExtendedKalmanFilter5D::updateCamera(const CamMeasurementVector& z_cam)
153152
{
154153
/* Predicted measurement. */
155-
CamMeasurementVector z_pred = cameraModel(m_state);
156-
Eigen::Matrix<float, CAM_MEAS_DIM, STATE_DIM> H = cameraJacobianH(m_state);
154+
CamMeasurementVector z_pred = cameraModel(m_state);
155+
Eigen::Matrix<float, CAM_MEAS_DIM, STATE_DIM> H = cameraJacobianH(m_state);
157156

158157
/* Innovation. */
159158
CamMeasurementVector y = z_cam - z_pred;
@@ -162,9 +161,8 @@ void ExtendedKalmanFilter5D::updateCamera(const CamMeasurementVector& z_cam)
162161
y(2) = wrapAngleMrad(y(2));
163162

164163
/* EKF update. */
165-
const CamMeasMatrix S = H * m_covariance * H.transpose() + m_R_cam;
166-
const Eigen::Matrix<float, STATE_DIM, CAM_MEAS_DIM> K =
167-
m_covariance * H.transpose() * S.inverse();
164+
const CamMeasMatrix S = H * m_covariance * H.transpose() + m_R_cam;
165+
const Eigen::Matrix<float, STATE_DIM, CAM_MEAS_DIM> K = m_covariance * H.transpose() * S.inverse();
168166

169167
m_state = m_state + K * y;
170168
m_state(2) = wrapAngleMrad(m_state(2));
@@ -174,8 +172,8 @@ void ExtendedKalmanFilter5D::updateCamera(const CamMeasurementVector& z_cam)
174172
void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasurementVector& z_odo)
175173
{
176174
/* Predicted measurement. */
177-
OdoMeasurementVector z_pred = odometryModel(m_state);
178-
Eigen::Matrix<float, ODO_MEAS_DIM, STATE_DIM> H = odometryJacobianH(m_state);
175+
OdoMeasurementVector z_pred = odometryModel(m_state);
176+
Eigen::Matrix<float, ODO_MEAS_DIM, STATE_DIM> H = odometryJacobianH(m_state);
179177

180178
/* Innovation. */
181179
OdoMeasurementVector y = z_odo - z_pred;
@@ -184,9 +182,8 @@ void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasurementVector& z_odo)
184182
y(3) = wrapAngleMrad(y(3));
185183

186184
/* EKF update. */
187-
const OdoMeasMatrix S = H * m_covariance * H.transpose() + m_R_odo;
188-
const Eigen::Matrix<float, STATE_DIM, ODO_MEAS_DIM> K =
189-
m_covariance * H.transpose() * S.inverse();
185+
const OdoMeasMatrix S = H * m_covariance * H.transpose() + m_R_odo;
186+
const Eigen::Matrix<float, STATE_DIM, ODO_MEAS_DIM> K = m_covariance * H.transpose() * S.inverse();
190187

191188
m_state = m_state + K * y;
192189
m_state(2) = wrapAngleMrad(m_state(2));
@@ -196,15 +193,14 @@ void ExtendedKalmanFilter5D::updateOdometry(const OdoMeasurementVector& z_odo)
196193
void ExtendedKalmanFilter5D::updateImu(const ImuMeasurementVector& z_imu)
197194
{
198195
/* Predicted measurement. */
199-
ImuMeasurementVector z_pred = imuModel(m_state);
200-
Eigen::Matrix<float, IMU_MEAS_DIM, STATE_DIM> H = imuJacobianH(m_state);
196+
ImuMeasurementVector z_pred = imuModel(m_state);
197+
Eigen::Matrix<float, IMU_MEAS_DIM, STATE_DIM> H = imuJacobianH(m_state);
201198
/* Innovation (omega, no wrapping). */
202199
ImuMeasurementVector y = z_imu - z_pred;
203200

204201
/* EKF update. */
205-
const ImuMeasMatrix S = H * m_covariance * H.transpose() + m_R_imu;
206-
const Eigen::Matrix<float, STATE_DIM, IMU_MEAS_DIM> K =
207-
m_covariance * H.transpose() * S.inverse();
202+
const ImuMeasMatrix S = H * m_covariance * H.transpose() + m_R_imu;
203+
const Eigen::Matrix<float, STATE_DIM, IMU_MEAS_DIM> K = m_covariance * H.transpose() * S.inverse();
208204

209205
m_state = m_state + K * y;
210206
m_state(2) = wrapAngleMrad(m_state(2));
@@ -215,8 +211,7 @@ void ExtendedKalmanFilter5D::updateImuFromDigits(int16_t rawGyroZ)
215211
{
216212
ImuMeasurementVector z_imu;
217213
/* Convert raw gyro digits to physical yaw rate [mrad/s]. */
218-
z_imu(0) = static_cast<float>(rawGyroZ) *
219-
SensorConstants::GYRO_SENSITIVITY_FACTOR;
214+
z_imu(0) = static_cast<float>(rawGyroZ) * SensorConstants::GYRO_SENSITIVITY_FACTOR;
220215

221216
updateImu(z_imu);
222217
}
@@ -225,31 +220,28 @@ void ExtendedKalmanFilter5D::updateImuFromDigits(int16_t rawGyroZ)
225220
* Private Methods
226221
*****************************************************************************/
227222

228-
ExtendedKalmanFilter5D::StateVector
229-
ExtendedKalmanFilter5D::processModel(const StateVector& x,
230-
float a_x_mms,
231-
float dt) const
223+
ExtendedKalmanFilter5D::StateVector ExtendedKalmanFilter5D::processModel(const StateVector& x, float a_x_mms,
224+
float dt) const
232225
{
233-
const float px = x(0);
234-
const float py = x(1);
235-
const float thetaMrad = x(2);
236-
const float v = x(3);
226+
const float px = x(0);
227+
const float py = x(1);
228+
const float thetaMrad = x(2);
229+
const float v = x(3);
237230
const float omegaMrads = x(4);
238231

239-
const float thetaRad = thetaMrad / 1000.0F;
232+
const float thetaRad = thetaMrad / 1000.0F;
240233

241234
StateVector x_next;
242235
x_next(0) = px + v * std::cos(thetaRad) * dt; /* p_x */
243236
x_next(1) = py + v * std::sin(thetaRad) * dt; /* p_y */
244-
x_next(2) = thetaMrad + omegaMrads * dt; /* theta [mrad] */
237+
x_next(2) = thetaMrad + omegaMrads * dt; /* theta [mrad] */
245238
x_next(3) = v + a_x_mms * dt; /* v */
246-
x_next(4) = omegaMrads; /* omega */
239+
x_next(4) = omegaMrads; /* omega */
247240

248241
return x_next;
249242
}
250243

251-
ExtendedKalmanFilter5D::StateMatrix
252-
ExtendedKalmanFilter5D::processJacobianF(const StateVector& x, float dt) const
244+
ExtendedKalmanFilter5D::StateMatrix ExtendedKalmanFilter5D::processJacobianF(const StateVector& x, float dt) const
253245
{
254246
const float thetaMrad = x(2);
255247
const float v = x(3);
@@ -260,12 +252,12 @@ ExtendedKalmanFilter5D::processJacobianF(const StateVector& x, float dt) const
260252
/* d p_x / d theta. */
261253
F_jacobian(0, 2) = -v * std::sin(thetaRad) * dt / 1000.0F;
262254
/* d p_x / d v. */
263-
F_jacobian(0, 3) = std::cos(thetaRad) * dt;
255+
F_jacobian(0, 3) = std::cos(thetaRad) * dt;
264256

265257
/* d p_y / d theta. */
266-
F_jacobian(1, 2) = v * std::cos(thetaRad) * dt / 1000.0F;
258+
F_jacobian(1, 2) = v * std::cos(thetaRad) * dt / 1000.0F;
267259
/* d p_y / d v. */
268-
F_jacobian(1, 3) = std::sin(thetaRad) * dt;
260+
F_jacobian(1, 3) = std::sin(thetaRad) * dt;
269261

270262
/* d theta / d omega. */
271263
F_jacobian(2, 4) = dt;
@@ -275,15 +267,14 @@ ExtendedKalmanFilter5D::processJacobianF(const StateVector& x, float dt) const
275267

276268
/* ---------------- Camera model ---------------- */
277269

278-
ExtendedKalmanFilter5D::CamMeasurementVector
279-
ExtendedKalmanFilter5D::cameraModel(const StateVector& x) const
270+
ExtendedKalmanFilter5D::CamMeasurementVector ExtendedKalmanFilter5D::cameraModel(const StateVector& x) const
280271
{
281272
const float px = x(0);
282273
const float py = x(1);
283274
const float thetaMrad = x(2);
284275
const float v = x(3);
285276

286-
const float thetaRad = thetaMrad / 1000.0F;
277+
const float thetaRad = thetaMrad / 1000.0F;
287278

288279
CamMeasurementVector z;
289280
z(0) = px; /* p_x */
@@ -294,8 +285,8 @@ ExtendedKalmanFilter5D::cameraModel(const StateVector& x) const
294285
return z;
295286
}
296287

297-
Eigen::Matrix<float, ExtendedKalmanFilter5D::CAM_MEAS_DIM, ExtendedKalmanFilter5D::STATE_DIM>
298-
ExtendedKalmanFilter5D::cameraJacobianH(const StateVector& x) const
288+
Eigen::Matrix<float, ExtendedKalmanFilter5D::CAM_MEAS_DIM, ExtendedKalmanFilter5D::STATE_DIM> ExtendedKalmanFilter5D::
289+
cameraJacobianH(const StateVector& x) const
299290
{
300291
const float thetaMrad = x(2);
301292
const float v = x(3);
@@ -313,19 +304,18 @@ ExtendedKalmanFilter5D::cameraJacobianH(const StateVector& x) const
313304

314305
/* v_x = v cos(theta) */
315306
H(3, 2) = -v * std::sin(thetaRad) * (1.0F / 1000.0F); /* d v_x / d theta */
316-
H(3, 3) = std::cos(thetaRad); /* d v_x / d v */
307+
H(3, 3) = std::cos(thetaRad); /* d v_x / d v */
317308

318309
/* v_y = v sin(theta) */
319-
H(4, 2) = v * std::cos(thetaRad) * (1.0F / 1000.0F); /* d v_y / d theta */
320-
H(4, 3) = std::sin(thetaRad); /* d v_y / d v */
310+
H(4, 2) = v * std::cos(thetaRad) * (1.0F / 1000.0F); /* d v_y / d theta */
311+
H(4, 3) = std::sin(thetaRad); /* d v_y / d v */
321312

322313
return H;
323314
}
324315

325316
/* ---------------- Odometry model ---------------- */
326317

327-
ExtendedKalmanFilter5D::OdoMeasurementVector
328-
ExtendedKalmanFilter5D::odometryModel(const StateVector& x) const
318+
ExtendedKalmanFilter5D::OdoMeasurementVector ExtendedKalmanFilter5D::odometryModel(const StateVector& x) const
329319
{
330320
OdoMeasurementVector z;
331321
z.setZero();
@@ -338,9 +328,8 @@ ExtendedKalmanFilter5D::odometryModel(const StateVector& x) const
338328
return z;
339329
}
340330

341-
342-
Eigen::Matrix<float, ExtendedKalmanFilter5D::ODO_MEAS_DIM, ExtendedKalmanFilter5D::STATE_DIM>
343-
ExtendedKalmanFilter5D::odometryJacobianH(const StateVector& /*x*/) const
331+
Eigen::Matrix<float, ExtendedKalmanFilter5D::ODO_MEAS_DIM, ExtendedKalmanFilter5D::STATE_DIM> ExtendedKalmanFilter5D::
332+
odometryJacobianH(const StateVector& /*x*/) const
344333
{
345334
Eigen::Matrix<float, ODO_MEAS_DIM, STATE_DIM> H;
346335
H.setZero();
@@ -359,16 +348,15 @@ ExtendedKalmanFilter5D::odometryJacobianH(const StateVector& /*x*/) const
359348

360349
/* ---------------- IMU yaw model ---------------- */
361350

362-
ExtendedKalmanFilter5D::ImuMeasurementVector
363-
ExtendedKalmanFilter5D::imuModel(const StateVector& x) const
351+
ExtendedKalmanFilter5D::ImuMeasurementVector ExtendedKalmanFilter5D::imuModel(const StateVector& x) const
364352
{
365353
ImuMeasurementVector z;
366354
z(0) = x(4); /* omega [mrad/s] */
367355
return z;
368356
}
369357

370-
Eigen::Matrix<float, ExtendedKalmanFilter5D::IMU_MEAS_DIM, ExtendedKalmanFilter5D::STATE_DIM>
371-
ExtendedKalmanFilter5D::imuJacobianH(const StateVector& /*x*/) const
358+
Eigen::Matrix<float, ExtendedKalmanFilter5D::IMU_MEAS_DIM, ExtendedKalmanFilter5D::STATE_DIM> ExtendedKalmanFilter5D::
359+
imuJacobianH(const StateVector& /*x*/) const
372360
{
373361
Eigen::Matrix<float, IMU_MEAS_DIM, STATE_DIM> H;
374362
H.setZero();

0 commit comments

Comments
 (0)