Skip to content

Commit 1119fcc

Browse files
Al-khwarizmi-780Al-khwarizmi-780
authored
Adding Motion Models (#19)
* adding CV motion model * fix build error * Add CA motion model * clean up * add CT motion model class * improve process noise covariance * improve process vector setting * adding tests for CV motion model * Add tests for CA motion model * Add tests for CT motion model * improve setting of sigma * Add parameterized tests
1 parent ee16dd4 commit 1119fcc

17 files changed

Lines changed: 1184 additions & 112 deletions

‎examples/ego_motion_model_adapter/main.cpp‎

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ class EgoMotionModelAdapter
3131
{
3232
public:
3333
Vector<DIM_X> f(Vector<DIM_X> const& vecX, Vector<DIM_U> const& vecU,
34-
Vector<DIM_X> const& /*vecQ = Vector<DIM_X>::Zero()*/) const
34+
float32_t dt) const
3535
{
3636
Vector<3> tmpVecX; // \vec{x} = [x, y, yaw]^T
3737
tmpVecX << vecX[0], vecX[1], vecX[4];
@@ -51,7 +51,8 @@ class EgoMotionModelAdapter
5151
}
5252

5353
Matrix<DIM_X, DIM_X> getProcessNoiseCov(Vector<DIM_X> const& vecX,
54-
Vector<DIM_U> const& vecU) const
54+
Vector<DIM_U> const& vecU,
55+
float32_t dt) const
5556
{
5657
// input idx -> output index mapping
5758
// 0 -> 0
@@ -91,7 +92,8 @@ class EgoMotionModelAdapter
9192
}
9293

9394
Matrix<DIM_X, DIM_X> getInputNoiseCov(Vector<DIM_X> const& vecX,
94-
Vector<DIM_U> const& vecU) const
95+
Vector<DIM_U> const& vecU,
96+
float32_t dt) const
9597
{
9698
Vector<3> tmpVecX;
9799
tmpVecX << vecX[0], vecX[1], vecX[4];
@@ -118,7 +120,8 @@ class EgoMotionModelAdapter
118120
}
119121

120122
Matrix<DIM_X, DIM_X> getJacobianFk(Vector<DIM_X> const& vecX,
121-
Vector<DIM_U> const& vecU) const
123+
Vector<DIM_U> const& vecU,
124+
float32_t dt) const
122125
{
123126
Vector<3> tmpVecX;
124127
tmpVecX << vecX[0], vecX[1], vecX[4];
@@ -145,7 +148,8 @@ class EgoMotionModelAdapter
145148
}
146149

147150
Matrix<DIM_X, DIM_U> getJacobianBk(Vector<DIM_X> const& vecX,
148-
Vector<DIM_U> const& vecU) const
151+
Vector<DIM_U> const& vecU,
152+
float32_t dt) const
149153
{
150154
Vector<3> tmpVecX;
151155
tmpVecX << vecX[0], vecX[1], vecX[4];

‎src/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,9 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON)
1919
set(LIBRARY_SRC_FILES
2020
dummy.cpp
2121
motion_model/ego_motion_model.cpp
22+
motion_model/cv_motion_model.cpp
23+
motion_model/ca_motion_model.cpp
24+
motion_model/ct_motion_model.cpp
2225
)
2326

2427
set(LIBRARY_HDR_FILES
@@ -30,6 +33,9 @@ set(LIBRARY_HDR_FILES
3033
kalman_filter/square_root_ukf.h
3134
motion_model/motion_model.h
3235
motion_model/ego_motion_model.h
36+
motion_model/cv_motion_model.h
37+
motion_model/ca_motion_model.h
38+
motion_model/ct_motion_model.h
3339
)
3440

3541
set(LIBRARY_NAME ${PROJECT_NAME})

‎src/kalman_filter/kalman_filter.h‎

Lines changed: 15 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -97,13 +97,16 @@ class KalmanFilter
9797
///
9898
/// @brief predict state with a linear process model.
9999
/// @param motionModel prediction motion model function
100+
/// @param dt time step between state updates (unit: seconds)
100101
///
101102
template <class Derived>
102-
void predictEkf(motionmodel::MotionModel<Derived, DIM_X> const& motionModel)
103+
void predictEkf(motionmodel::MotionModel<Derived, DIM_X> const& motionModel,
104+
float32_t dt = 1.0F)
103105
{
104-
Matrix<DIM_X, DIM_X> const matFk{motionModel.getJacobianFk(m_vecX)};
105-
Matrix<DIM_X, DIM_X> const matQk{motionModel.getProcessNoiseCov(m_vecX)};
106-
m_vecX = motionModel.f(m_vecX);
106+
Matrix<DIM_X, DIM_X> const matFk{motionModel.getJacobianFk(m_vecX, dt)};
107+
Matrix<DIM_X, DIM_X> const matQk{
108+
motionModel.getProcessNoiseCov(m_vecX, dt)};
109+
m_vecX = motionModel.f(m_vecX, dt);
107110
m_matP = matFk * m_matP * matFk.transpose() + matQk;
108111

109112
// Ensure symmetry of covariance matrix
@@ -114,16 +117,20 @@ class KalmanFilter
114117
/// @brief predict state with a linear process model with external input.
115118
/// @param motionModel prediction motion model function
116119
/// @param vecU input vector
120+
/// @param dt time step between state updates (unit: seconds)
117121
///
118122
template <class Derived, int32_t DIM_U>
119123
void predictEkf(motionmodel::MotionModelExtInput<Derived, DIM_X, DIM_U> const&
120124
motionModel,
121-
Vector<DIM_U> const& vecU)
125+
Vector<DIM_U> const& vecU, float32_t dt = 1.0F)
122126
{
123-
Matrix<DIM_X, DIM_X> const matFk{motionModel.getJacobianFk(m_vecX, vecU)};
127+
Matrix<DIM_X, DIM_X> const matFk{
128+
motionModel.getJacobianFk(m_vecX, vecU, dt)};
129+
124130
Matrix<DIM_X, DIM_X> const matQk{
125-
motionModel.getProcessNoiseCov(m_vecX, vecU)};
126-
m_vecX = motionModel.f(m_vecX, vecU);
131+
motionModel.getProcessNoiseCov(m_vecX, vecU, dt)};
132+
133+
m_vecX = motionModel.f(m_vecX, vecU, dt);
127134
m_matP = matFk * m_matP * matFk.transpose() + matQk;
128135

129136
// Ensure symmetry of covariance matrix

‎src/motion_model/ca_motion_model.cpp‎

Lines changed: 158 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,158 @@
1+
#include "ca_motion_model.h"
2+
3+
namespace kf
4+
{
5+
namespace motionmodel
6+
{
7+
8+
Vector<DIM_X_CA> CaMotionModel::f(Vector<DIM_X_CA> const& vecX,
9+
float32_t dt) const
10+
{
11+
// State transition model for constant acceleration (CA) motion model
12+
// [ pos_x ] [ 1 0 dt 0 dt^2/2 0 ] [ pos_x ] [ q1 ]
13+
// [ pos_y ] = [ 0 1 0 dt 0 dt^2/2 ] [ pos_y ] + [ q2 ]
14+
// [ vel_x ] [ 0 0 1 0 dt 0 ] [ vel_x ] [ q3 ]
15+
// [ vel_y ] [ 0 0 0 1 0 dt ] [ vel_y ] [ q4 ]
16+
// [ acc_x ] [ 0 0 0 0 1 0 ] [ acc_x ] [ q5 ]
17+
// [ acc_y ] [ 0 0 0 0 0 1 ] [ acc_y ] [ q6 ]
18+
19+
float32_t const halfDeltaT2{dt * dt / 2.0F};
20+
21+
Vector<DIM_X_CA> vecXPred;
22+
vecXPred[IDX_PX] =
23+
vecX[IDX_PX] + vecX[IDX_VX] * dt + vecX[IDX_AX] * halfDeltaT2;
24+
vecXPred[IDX_PY] =
25+
vecX[IDX_PY] + vecX[IDX_VY] * dt + vecX[IDX_AY] * halfDeltaT2;
26+
vecXPred[IDX_VX] = vecX[IDX_VX] + vecX[IDX_AX] * dt;
27+
vecXPred[IDX_VY] = vecX[IDX_VY] + vecX[IDX_AY] * dt;
28+
vecXPred[IDX_AX] = vecX[IDX_AX];
29+
vecXPred[IDX_AY] = vecX[IDX_AY];
30+
31+
return vecXPred;
32+
}
33+
34+
Matrix<DIM_X_CA, DIM_X_CA> CaMotionModel::getProcessNoiseCov(
35+
Vector<DIM_X_CA> const& /*vecX*/, float32_t dt) const
36+
{
37+
// Q = sigma^2*[T^5/20 0 T^4/8 0 T^3/6 0;
38+
// 0 T^5/20 0 T^4/8 0 T^3/6;
39+
// T^4/8 0 T^3/3 0 T^2/2 0;
40+
// 0 T^4/8 0 T^2/2 0 T^2/2;
41+
// T^3/6 0 T^2/2 0 T 0;
42+
// 0 T^3/6 0 T^2/2 0 T;
43+
// ];
44+
45+
Matrix<DIM_X_CA, DIM_X_CA> matQ;
46+
47+
const float32_t sigma2{m_processNoiseVec[0] * m_processNoiseVec[0]};
48+
const float32_t dt2{dt * dt};
49+
const float32_t dt3{dt2 * dt};
50+
const float32_t dt4{dt2 * dt2};
51+
const float32_t dt5{dt4 * dt};
52+
53+
matQ(IDX_PX, IDX_PX) = sigma2 * dt5 / 20.0F;
54+
matQ(IDX_PX, IDX_PY) = 0.0F;
55+
matQ(IDX_PX, IDX_VX) = sigma2 * dt4 / 8.0F;
56+
matQ(IDX_PX, IDX_VY) = 0.0F;
57+
matQ(IDX_PX, IDX_AX) = sigma2 * dt3 / 6.0F;
58+
matQ(IDX_PX, IDX_AY) = 0.0F;
59+
60+
matQ(IDX_PY, IDX_PX) = 0.0F;
61+
matQ(IDX_PY, IDX_PY) = sigma2 * dt5 / 20.0F;
62+
matQ(IDX_PY, IDX_VX) = 0.0F;
63+
matQ(IDX_PY, IDX_VY) = sigma2 * dt4 / 8.0F;
64+
matQ(IDX_PY, IDX_AX) = 0.0F;
65+
matQ(IDX_PY, IDX_AY) = sigma2 * dt3 / 6.0F;
66+
67+
matQ(IDX_VX, IDX_PX) = sigma2 * dt4 / 8.0F;
68+
matQ(IDX_VX, IDX_PY) = 0.0F;
69+
matQ(IDX_VX, IDX_VX) = sigma2 * dt3 / 3.0F;
70+
matQ(IDX_VX, IDX_VY) = 0.0F;
71+
matQ(IDX_VX, IDX_AX) = sigma2 * dt2 / 2.0F;
72+
matQ(IDX_VX, IDX_AY) = 0.0F;
73+
74+
matQ(IDX_VY, IDX_PX) = 0.0F;
75+
matQ(IDX_VY, IDX_PY) = sigma2 * dt4 / 8.0F;
76+
matQ(IDX_VY, IDX_VX) = 0.0F;
77+
matQ(IDX_VY, IDX_VY) = sigma2 * dt2 / 2.0F;
78+
matQ(IDX_VY, IDX_AX) = 0.0F;
79+
matQ(IDX_VY, IDX_AY) = sigma2 * dt2 / 2.0F;
80+
81+
matQ(IDX_AX, IDX_PX) = sigma2 * dt3 / 6.0F;
82+
matQ(IDX_AX, IDX_PY) = 0.0F;
83+
matQ(IDX_AX, IDX_VX) = sigma2 * dt2 / 2.0F;
84+
matQ(IDX_AX, IDX_VY) = 0.0F;
85+
matQ(IDX_AX, IDX_AX) = sigma2 * dt;
86+
matQ(IDX_AX, IDX_AY) = 0.0F;
87+
88+
matQ(IDX_AY, IDX_PX) = 0.0F;
89+
matQ(IDX_AY, IDX_PY) = sigma2 * dt3 / 6.0F;
90+
matQ(IDX_AY, IDX_VX) = 0.0F;
91+
matQ(IDX_AY, IDX_VY) = sigma2 * dt2 / 2.0F;
92+
matQ(IDX_AY, IDX_AX) = 0.0F;
93+
matQ(IDX_AY, IDX_AY) = sigma2 * dt;
94+
95+
return matQ;
96+
}
97+
98+
Matrix<DIM_X_CA, DIM_X_CA> CaMotionModel::getJacobianFk(
99+
Vector<DIM_X_CA> const& vecX, float32_t dt) const
100+
{
101+
// State transition model for constant acceleration (CA) motion model
102+
// [ pos_x ] [ 1 0 dt 0 dt^2/2 0 ] [ pos_x ]
103+
// [ pos_y ] = [ 0 1 0 dt 0 dt^2/2 ] [ pos_y ]
104+
// [ vel_x ] [ 0 0 1 0 dt 0 ] [ vel_x ]
105+
// [ vel_y ] [ 0 0 0 1 0 dt ] [ vel_y ]
106+
// [ acc_x ] [ 0 0 0 0 1 0 ] [ acc_x ]
107+
// [ acc_y ] [ 0 0 0 0 0 1 ] [ acc_y ]
108+
109+
float32_t const halfdt2{0.5F * dt * dt};
110+
111+
Matrix<DIM_X_CA, DIM_X_CA> matFk;
112+
matFk(IDX_PX, IDX_PX) = 1.0F;
113+
matFk(IDX_PX, IDX_PY) = 0.0F;
114+
matFk(IDX_PX, IDX_VX) = dt;
115+
matFk(IDX_PX, IDX_VY) = 0.0F;
116+
matFk(IDX_PX, IDX_AX) = halfdt2;
117+
matFk(IDX_PX, IDX_AY) = 0.0F;
118+
119+
matFk(IDX_PY, IDX_PX) = 0.0F;
120+
matFk(IDX_PY, IDX_PY) = 1.0F;
121+
matFk(IDX_PY, IDX_VX) = 0.0F;
122+
matFk(IDX_PY, IDX_VY) = dt;
123+
matFk(IDX_PY, IDX_AX) = 0.0F;
124+
matFk(IDX_PY, IDX_AY) = halfdt2;
125+
126+
matFk(IDX_VX, IDX_PX) = 0.0F;
127+
matFk(IDX_VX, IDX_PY) = 0.0F;
128+
matFk(IDX_VX, IDX_VX) = 1.0F;
129+
matFk(IDX_VX, IDX_VY) = 0.0F;
130+
matFk(IDX_VX, IDX_AX) = dt;
131+
matFk(IDX_VX, IDX_AY) = 0.0F;
132+
133+
matFk(IDX_VY, IDX_PX) = 0.0F;
134+
matFk(IDX_VY, IDX_PY) = 0.0F;
135+
matFk(IDX_VY, IDX_VX) = 0.0F;
136+
matFk(IDX_VY, IDX_VY) = 1.0F;
137+
matFk(IDX_VY, IDX_AX) = 0.0F;
138+
matFk(IDX_VY, IDX_AY) = dt;
139+
140+
matFk(IDX_AX, IDX_PX) = 0.0F;
141+
matFk(IDX_AX, IDX_PY) = 0.0F;
142+
matFk(IDX_AX, IDX_VX) = 0.0F;
143+
matFk(IDX_AX, IDX_VY) = 0.0F;
144+
matFk(IDX_AX, IDX_AX) = 1.0F;
145+
matFk(IDX_AX, IDX_AY) = 0.0F;
146+
147+
matFk(IDX_AY, IDX_PX) = 0.0F;
148+
matFk(IDX_AY, IDX_PY) = 0.0F;
149+
matFk(IDX_AY, IDX_VX) = 0.0F;
150+
matFk(IDX_AY, IDX_VY) = 0.0F;
151+
matFk(IDX_AY, IDX_AX) = 0.0F;
152+
matFk(IDX_AY, IDX_AY) = 1.0F;
153+
154+
return matFk;
155+
}
156+
157+
} // namespace motionmodel
158+
} // namespace kf

‎src/motion_model/ca_motion_model.h‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
#include "motion_model.h"
2+
#include "types.h"
3+
4+
namespace kf
5+
{
6+
namespace motionmodel
7+
{
8+
/// @brief State space dimension for constant acceleration motion model
9+
/// \vec{x}=[pos_x, pos_y, vel_x, vel_y, acc_x, acc_y]^T
10+
static constexpr int32_t DIM_X_CA{6};
11+
12+
/// @brief Process noise dimension for constant acceleration motion model
13+
static constexpr int32_t DIM_Q_CA{1};
14+
15+
class CaMotionModel : public MotionModel<CaMotionModel, DIM_X_CA>
16+
{
17+
public:
18+
CaMotionModel(float32_t const sigma = 1.0F) { m_processNoiseVec[0] = sigma; }
19+
~CaMotionModel() {}
20+
21+
/// @brief Prediction motion model function that propagate the previous state
22+
/// to next state in time.
23+
/// @param vecX State space vector \vec{x}
24+
/// @param dt Time step between state updates (unit: seconds)
25+
/// @return Predicted/ propagated state space vector
26+
Vector<DIM_X_CA> f(Vector<DIM_X_CA> const& vecX, float32_t dt = 1.0F) const;
27+
28+
/// @brief Get the process noise covariance Q
29+
/// @param vecX State space vector \vec{x}
30+
/// @param dt Time step between state updates (unit: seconds)
31+
/// @return The process noise covariance Q
32+
Matrix<DIM_X_CA, DIM_X_CA> getProcessNoiseCov(Vector<DIM_X_CA> const& vecX,
33+
float32_t dt = 1.0F) const;
34+
35+
/// @brief Method that calculates the jacobians of the state transition model.
36+
/// @param vecX State Space vector \vec{x}
37+
/// @param dt Time step between state updates (unit: seconds)
38+
/// @return The jacobians of the state transition model.
39+
Matrix<DIM_X_CA, DIM_X_CA> getJacobianFk(Vector<DIM_X_CA> const& vecX,
40+
float32_t dt = 1.0F) const;
41+
42+
/// @brief Get state dimension
43+
/// @return State dimension
44+
static constexpr int32_t getStateDim() { return DIM_X_CA; }
45+
46+
/// @brief Set process noise standard deviation
47+
/// @param sigma Process noise standard deviation
48+
void setSigma(float32_t const sigma) { m_processNoiseVec[0] = sigma; }
49+
50+
static constexpr int32_t IDX_PX{0}; //< Index for position x
51+
static constexpr int32_t IDX_PY{1}; //< Index for position y
52+
static constexpr int32_t IDX_VX{2}; //< Index for velocity x
53+
static constexpr int32_t IDX_VY{3}; //< Index for velocity y
54+
static constexpr int32_t IDX_AX{4}; //< Index for acceleration x
55+
static constexpr int32_t IDX_AY{5}; //< Index for acceleration y
56+
57+
private:
58+
Vector<DIM_Q_CA> m_processNoiseVec; //< Process noise vector
59+
};
60+
61+
} // namespace motionmodel
62+
} // namespace kf

0 commit comments

Comments
 (0)