Skip to content

Commit f95017c

Browse files
Al-khwarizmi-780Al-khwarizmi-780
committed
improve process vector setting
1 parent 874d3c1 commit f95017c

7 files changed

Lines changed: 59 additions & 36 deletions

File tree

‎src/motion_model/ca_motion_model.cpp‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -31,9 +31,8 @@ Vector<DIM_X_CA> CaMotionModel::f(Vector<DIM_X_CA> const& vecX,
3131
return vecXPred;
3232
}
3333

34-
template <>
35-
Matrix<DIM_X_CA, DIM_X_CA> CaMotionModel::getProcessNoiseCov<1>(
36-
Vector<1> const& sigma, float32_t dt) const
34+
Matrix<DIM_X_CA, DIM_X_CA> CaMotionModel::getProcessNoiseCov(
35+
Vector<DIM_X_CA> const& /*vecX*/, float32_t dt) const
3736
{
3837
// Q = sigma^2*[T^5/20 0 T^4/8 0 T^3/6 0;
3938
// 0 T^5/20 0 T^4/8 0 T^3/6;
@@ -45,7 +44,7 @@ Matrix<DIM_X_CA, DIM_X_CA> CaMotionModel::getProcessNoiseCov<1>(
4544

4645
Matrix<DIM_X_CA, DIM_X_CA> matQ;
4746

48-
const float32_t sigma2{sigma[0] * sigma[0]};
47+
const float32_t sigma2{m_processNoiseVec[0] * m_processNoiseVec[0]};
4948
const float32_t dt2{dt * dt};
5049
const float32_t dt3{dt2 * dt};
5150
const float32_t dt4{dt2 * dt2};

‎src/motion_model/ca_motion_model.h‎

Lines changed: 14 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,9 @@ namespace motionmodel
99
/// \vec{x}=[pos_x, pos_y, vel_x, vel_y, acc_x, acc_y]^T
1010
static constexpr int32_t DIM_X_CA{6};
1111

12+
/// @brief Process noise dimension for constant acceleration motion model
13+
static constexpr int32_t DIM_Q_CA{1};
14+
1215
class CaMotionModel : public MotionModel<CaMotionModel, DIM_X_CA>
1316
{
1417
public:
@@ -23,11 +26,10 @@ class CaMotionModel : public MotionModel<CaMotionModel, DIM_X_CA>
2326
Vector<DIM_X_CA> f(Vector<DIM_X_CA> const& vecX, float32_t dt = 1.0F) const;
2427

2528
/// @brief Get the process noise covariance Q
26-
/// @param sigma Standard deviation of the process noise v=[sigma]
29+
/// @param vecX State space vector \vec{x}
2730
/// @param dt Time step between state updates (unit: seconds)
2831
/// @return The process noise covariance Q
29-
template <int32_t DIM_SIGMA>
30-
Matrix<DIM_X_CA, DIM_X_CA> getProcessNoiseCov(Vector<DIM_SIGMA> const& sigma,
32+
Matrix<DIM_X_CA, DIM_X_CA> getProcessNoiseCov(Vector<DIM_X_CA> const& vecX,
3133
float32_t dt = 1.0F) const;
3234

3335
/// @brief Method that calculates the jacobians of the state transition model.
@@ -41,13 +43,22 @@ class CaMotionModel : public MotionModel<CaMotionModel, DIM_X_CA>
4143
/// @return State dimension
4244
static constexpr int32_t getStateDim() { return DIM_X_CA; }
4345

46+
/// @brief Set process noise vector
47+
/// @param processNoiseVec Process noise vector
48+
void setProcessNoiseVector(Vector<DIM_Q_CA> const& processNoiseVec)
49+
{
50+
m_processNoiseVec = processNoiseVec;
51+
}
52+
4453
private:
4554
static constexpr int32_t IDX_PX{0}; //< Index for position x
4655
static constexpr int32_t IDX_PY{1}; //< Index for position y
4756
static constexpr int32_t IDX_VX{2}; //< Index for velocity x
4857
static constexpr int32_t IDX_VY{3}; //< Index for velocity y
4958
static constexpr int32_t IDX_AX{4}; //< Index for acceleration x
5059
static constexpr int32_t IDX_AY{5}; //< Index for acceleration y
60+
61+
Vector<DIM_Q_CA> m_processNoiseVec; //< Process noise vector
5162
};
5263

5364
} // namespace motionmodel

‎src/motion_model/ct_motion_model.cpp‎

Lines changed: 8 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -27,19 +27,13 @@ Vector<DIM_X_CT> CtMotionModel::f(Vector<DIM_X_CT> const& vecX,
2727
return vecXPred;
2828
}
2929

30-
template <>
31-
Matrix<DIM_X_CT, DIM_X_CT> CtMotionModel::getProcessNoiseCov<2>(
32-
Vector<2> const& sigma, float32_t dt) const
30+
Matrix<DIM_X_CT, DIM_X_CT> CtMotionModel::getProcessNoiseCov(
31+
Vector<DIM_X_CT> const& vecX, float32_t dt) const
3332
{
3433
Matrix<DIM_X_CT, DIM_X_CT> matQ{Matrix<DIM_X_CT, DIM_X_CT>::Zero()};
3534

36-
float32_t const sigma2{sigma[0] * sigma[0]};
37-
38-
matQ(IDX_V, IDX_V) = sigma2;
39-
matQ(IDX_OMEGA, IDX_OMEGA) = sigma2;
40-
41-
float32_t const sigma_accel2{sigma[0] * sigma[0]};
42-
float32_t const sigma_alpha2{sigma[1] * sigma[1]};
35+
float32_t const sigma_accel2{m_processNoiseVec[0] * m_processNoiseVec[0]};
36+
float32_t const sigma_alpha2{m_processNoiseVec[1] * m_processNoiseVec[1]};
4337

4438
float32_t const dt2{dt * dt};
4539
float32_t const dt3{dt2 * dt};
@@ -49,18 +43,18 @@ Matrix<DIM_X_CT, DIM_X_CT> CtMotionModel::getProcessNoiseCov<2>(
4943

5044
matQ(IDX_PX, IDX_PX) = sigma_accel2 * dt3_div3;
5145
matQ(IDX_PX, IDX_PY) = 0.0F;
52-
matQ(IDX_PX, IDX_V) = sigma_accel2 * dt2_div2 /* *cosf(vecX[IDX_THETA]) */;
46+
matQ(IDX_PX, IDX_V) = sigma_accel2 * dt2_div2 * cosf(vecX[IDX_THETA]);
5347
matQ(IDX_PX, IDX_THETA) = 0.0F;
5448
matQ(IDX_PX, IDX_OMEGA) = 0.0F;
5549

5650
matQ(IDX_PY, IDX_PX) = 0.0F;
5751
matQ(IDX_PY, IDX_PY) = sigma_accel2 * dt3_div3;
58-
matQ(IDX_PY, IDX_V) = sigma_accel2 * dt2_div2 /* *sinf(vecX[IDX_THETA]) */;
52+
matQ(IDX_PY, IDX_V) = sigma_accel2 * dt2_div2 * sinf(vecX[IDX_THETA]);
5953
matQ(IDX_PY, IDX_THETA) = 0.0F;
6054
matQ(IDX_PY, IDX_OMEGA) = 0.0F;
6155

62-
matQ(IDX_V, IDX_PX) = sigma_accel2 * dt2_div2 /* *cosf(vecX[IDX_THETA]) */;
63-
matQ(IDX_V, IDX_PY) = sigma_accel2 * dt2_div2 /* *sinf(vecX[IDX_THETA]) */;
56+
matQ(IDX_V, IDX_PX) = sigma_accel2 * dt2_div2 * cosf(vecX[IDX_THETA]);
57+
matQ(IDX_V, IDX_PY) = sigma_accel2 * dt2_div2 * sinf(vecX[IDX_THETA]);
6458
matQ(IDX_V, IDX_V) = sigma_accel2 * dt;
6559
matQ(IDX_V, IDX_THETA) = 0.0F;
6660
matQ(IDX_V, IDX_OMEGA) = 0.0F;

‎src/motion_model/ct_motion_model.h‎

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,9 @@ namespace motionmodel
99
/// \vec{x}=[pos_x, pos_y, v, theta, omega]^T
1010
static constexpr int32_t DIM_X_CT{5};
1111

12+
/// @brief Process noise dimension for constant turn rate motion model
13+
static constexpr int32_t DIM_Q_CT{2};
14+
1215
class CtMotionModel : public MotionModel<CtMotionModel, DIM_X_CT>
1316
{
1417
public:
@@ -23,12 +26,10 @@ class CtMotionModel : public MotionModel<CtMotionModel, DIM_X_CT>
2326
Vector<DIM_X_CT> f(Vector<DIM_X_CT> const& vecX, float32_t dt = 1.0F) const;
2427

2528
/// @brief Get the process noise covariance Q
26-
/// @param sigma Standard deviation of the process noise v=[sigma_a,
27-
/// sigma_alpha]
29+
/// @param vecX State space vector \vec{x}
2830
/// @param dt Time step between state updates (unit: seconds)
2931
/// @return The process noise covariance Q
30-
template <int32_t DIM_SIGMA>
31-
Matrix<DIM_X_CT, DIM_X_CT> getProcessNoiseCov(Vector<DIM_SIGMA> const& sigma,
32+
Matrix<DIM_X_CT, DIM_X_CT> getProcessNoiseCov(Vector<DIM_X_CT> const& vecX,
3233
float32_t dt = 1.0F) const;
3334

3435
/// @brief Method that calculates the jacobians of the state transition model.
@@ -42,12 +43,21 @@ class CtMotionModel : public MotionModel<CtMotionModel, DIM_X_CT>
4243
/// @return State dimension
4344
static constexpr int32_t getStateDim() { return DIM_X_CT; }
4445

46+
/// @brief Set process noise vector
47+
/// @param processNoiseVec Process noise vector
48+
void setProcessNoiseVector(Vector<DIM_Q_CT> const& processNoiseVec)
49+
{
50+
m_processNoiseVec = processNoiseVec;
51+
}
52+
4553
private:
4654
static constexpr int32_t IDX_PX{0}; //< Index for position x
4755
static constexpr int32_t IDX_PY{1}; //< Index for position y
4856
static constexpr int32_t IDX_V{2}; //< Index for velocity
4957
static constexpr int32_t IDX_THETA{3}; //< Index for heading angle
5058
static constexpr int32_t IDX_OMEGA{4}; //< Index for yaw rate
59+
60+
Vector<DIM_Q_CT> m_processNoiseVec; //< Process noise vector
5161
};
5262

5363
} // namespace motionmodel

‎src/motion_model/cv_motion_model.cpp‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -24,9 +24,8 @@ Vector<DIM_X_CV> CvMotionModel::f(Vector<DIM_X_CV> const& vecX,
2424
return vecXPred;
2525
}
2626

27-
template <>
28-
Matrix<DIM_X_CV, DIM_X_CV> CvMotionModel::getProcessNoiseCov<1>(
29-
Vector<1> const& sigma, float32_t dt) const
27+
Matrix<DIM_X_CV, DIM_X_CV> CvMotionModel::getProcessNoiseCov(
28+
Vector<DIM_X_CV> const& /*vecX*/, float32_t dt) const
3029
{
3130
// Q = sigma^2*[T^4/4 0 T^3/2 0;
3231
// 0 T^4/4 0 T^3/2;
@@ -36,7 +35,7 @@ Matrix<DIM_X_CV, DIM_X_CV> CvMotionModel::getProcessNoiseCov<1>(
3635

3736
Matrix<DIM_X_CV, DIM_X_CV> matQ;
3837

39-
const float32_t sigma2 = sigma[0] * sigma[0];
38+
const float32_t sigma2 = m_processNoiseVec[0] * m_processNoiseVec[0];
4039
const float32_t dt2 = dt * dt;
4140
const float32_t dt3 = dt2 * dt;
4241
const float32_t dt4 = dt2 * dt2;

‎src/motion_model/cv_motion_model.h‎

Lines changed: 14 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,9 @@ namespace motionmodel
99
/// \vec{x}=[pos_x, pos_y, vel_x, vel_y]^T
1010
static constexpr int32_t DIM_X_CV{4};
1111

12+
/// @brief Process noise dimension for constant velocity motion model
13+
static constexpr int32_t DIM_Q_CV{1};
14+
1215
class CvMotionModel : public MotionModel<CvMotionModel, DIM_X_CV>
1316
{
1417
public:
@@ -23,11 +26,10 @@ class CvMotionModel : public MotionModel<CvMotionModel, DIM_X_CV>
2326
Vector<DIM_X_CV> f(Vector<DIM_X_CV> const& vecX, float32_t dt = 1.0F) const;
2427

2528
/// @brief Get the process noise covariance Q
26-
/// @param sigma Standard deviation of the process noise v=[sigma]
29+
/// @param vecX State space vector \vec{x}
2730
/// @param dt Time step between state updates (unit: seconds)
2831
/// @return The process noise covariance Q
29-
template <int32_t DIM_SIGMA>
30-
Matrix<DIM_X_CV, DIM_X_CV> getProcessNoiseCov(Vector<DIM_SIGMA> const& sigma,
32+
Matrix<DIM_X_CV, DIM_X_CV> getProcessNoiseCov(Vector<DIM_X_CV> const& vecX,
3133
float32_t dt = 1.0F) const;
3234

3335
/// @brief Method that calculates the jacobians of the state transition model.
@@ -41,11 +43,20 @@ class CvMotionModel : public MotionModel<CvMotionModel, DIM_X_CV>
4143
/// @return State dimension
4244
static constexpr int32_t getStateDim() { return DIM_X_CV; }
4345

46+
/// @brief Set process noise vector
47+
/// @param processNoiseVec Process noise vector
48+
void setProcessNoiseVector(Vector<DIM_Q_CV> const& processNoiseVec)
49+
{
50+
m_processNoiseVec = processNoiseVec;
51+
}
52+
4453
private:
4554
static constexpr int32_t IDX_PX{0}; //< Index for position x
4655
static constexpr int32_t IDX_PY{1}; //< Index for position y
4756
static constexpr int32_t IDX_VX{2}; //< Index for velocity x
4857
static constexpr int32_t IDX_VY{3}; //< Index for velocity y
58+
59+
Vector<DIM_Q_CV> m_processNoiseVec; //< Process noise vector
4960
};
5061

5162
} // namespace motionmodel

‎src/motion_model/motion_model.h‎

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -25,14 +25,13 @@ class MotionModel
2525
}
2626

2727
/// @brief Get the process noise covariance Q
28-
/// @param sigma Vector constaining standard deviations of the process noise
28+
/// @param vecX State space vector \vec{x}
2929
/// @param dt Time step between state updates (unit: seconds)
3030
/// @return The process noise covariance Q
31-
template <int32_t DIM_SIGMA>
32-
Matrix<DIM_X, DIM_X> getProcessNoiseCov(Vector<DIM_SIGMA> const& sigma,
31+
Matrix<DIM_X, DIM_X> getProcessNoiseCov(Vector<DIM_X> const& vecX,
3332
float32_t dt = 1.0F) const
3433
{
35-
return static_cast<Derived const*>(this)->getProcessNoiseCov(sigma, dt);
34+
return static_cast<Derived const*>(this)->getProcessNoiseCov(vecX, dt);
3635
}
3736

3837
/// @brief Method that calculates the jacobians of the state transition model.

0 commit comments

Comments
 (0)