@@ -9,21 +9,21 @@ CTRV model is a model that assumes constant turn rate and velocity magnitude.
99- state transition equation
1010
1111$$
12- \begin{align* }
13- x_{k+1} &= x_k + v_{x_k } \cos(\psi_k) \cdot dt \\
14- y_{k+1} &= y_k + v_{x_k } \sin(\psi_k) \cdot dt \\
15- \psi_{k+1} &= \psi_k + \dot{\psi}_k \cdot dt \\
16- v_{x_{ k+1}} & = v_{x_k } \\
17- \dot{\psi}_ {k+1} & = \dot{\psi}_k \\
18- \end{align* }
12+ \begin{aligned }
13+ x_{k+1} & = x_{k} + v_{k } \cos(\psi_k) \cdot {d t} \\
14+ y_{k+1} & = y_{k} + v_{k } \sin(\psi_k) \cdot {d t} \\
15+ \psi_{k+1} & = \psi_k + \dot\psi_{k} \cdot {d t} \\
16+ v_{k+1} & = v_{k } \\
17+ \dot\psi_ {k+1} & = \dot\psi_{k} \\
18+ \end{aligned }
1919$$
2020
2121- jacobian
2222
2323$$
2424A = \begin{bmatrix}
25- 1 & 0 & -v_x \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\
26- 0 & 1 & v_x \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\
25+ 1 & 0 & -v \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\
26+ 0 & 1 & v \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\
27270 & 0 & 1 & 0 & dt \\
28280 & 0 & 0 & 1 & 0 \\
29290 & 0 & 0 & 0 & 1 \\
@@ -38,17 +38,20 @@ The merit of using this model is that it can prevent unintended yaw rotation whe
3838![ kinematic_bicycle_model] ( image/kinematic_bicycle_model.png )
3939
4040- ** state variable**
41- - pose( $x,y$ ), velocity ( $v $ ), yaw ( $\psi $ ), and slip angle ( $\beta$ )
42- - $[ x_ {k} , y_ {k} , v _ {k} , \psi _ {k} , \beta_ {k} ] ^\mathrm{T}$
41+ - pose( $x,y$ ), yaw ( $\psi $ ), velocity ( $v $ ), and slip angle ( $\beta$ )
42+ - $[ x_ {k}, y_ {k}, \psi _ {k}, v _ {k}, \beta_ {k} ] ^\mathrm{T}$
4343- ** Prediction Equation**
4444 - $dt$: sampling time
45+ - $w_ {k} = \dot\psi_ {k} = \frac{ v_ {k} \sin \left( \beta_ {k} \right) }{ l_r }$ : angular velocity
4546
4647$$
4748\begin{aligned}
48- x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\
49- y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\
50- v_{k+1} & =v_{k} \\
51- \psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\
49+ x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t}
50+ -\frac{1}{2} \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\
51+ y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t}
52+ +\frac{1}{2} \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\
53+ \psi_{k+1} & =\psi_{k} + w_k {d t} \\
54+ v_{k+1} & = v_{k} \\
5255\beta_{k+1} & =\beta_{k}
5356\end{aligned}
5457$$
5760
5861$$
5962\frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc}
60- 1 & 0 & -v \sin (\psi+\beta) d t & v \cos (\psi+\beta) & -v \sin (\psi+\beta) d t \\
61- 0 & 1 & v \cos (\psi+\beta) d t & v \sin (\psi+\beta) & v \cos (\psi+\beta) d t \\
62- 0 & 0 & 1 & \frac{1}{l_r} \sin \beta d t & \frac{v}{l_r} \cos \beta d t \\
63+ 1 & 0
64+ & v \cos (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \cos \left( \psi+\beta \right) \right\rbrace {d t}^2
65+ & \sin (\psi+\beta) {d t} - \left\lbrace w \sin \left( \psi+\beta \right) \right\rbrace {d t}^2
66+ & -v \sin (\psi+\beta) {d t} - \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\sin(\psi+\beta)+\sin(\beta)\cos(\psi+\beta) \right\rbrace {d t}^2 \\
67+ 0 & 1
68+ & v \sin (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \sin \left( \psi+\beta \right) \right\rbrace {d t}^2
69+ & \cos (\psi+\beta) {d t} + \left\lbrace w \cos \left( \psi+\beta \right) \right\rbrace {d t}^2
70+ & v \cos (\psi+\beta) {d t} + \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\cos(\psi+\beta)-\sin(\beta)\sin(\psi+\beta) \right\rbrace {d t}^2 \\
71+ 0 & 0 & 1 & \frac{1}{l_r} \sin \beta {d t} & \frac{v}{l_r} \cos \beta d t \\
63720 & 0 & 0 & 1 & 0 \\
64730 & 0 & 0 & 0 & 1
6574\end{array}\right]
![pre-commit-ci[bot]](https://avatars.githubusercontent.com/in/68672?v=4&size=40){kind=avatar}
0 commit comments