|
35 | 35 | "\n",
|
36 | 36 | "In the code, \"xEst\" means the state vector. [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L168)\n",
|
37 | 37 | "\n",
|
| 38 | + "And, $P_t$ is covariace matrix of the state,\n", |
| 39 | + "\n", |
| 40 | + "$Q$ is covariance matrix of process noise, \n", |
| 41 | + "\n", |
| 42 | + "$R$ is covariance matrix of observation noise at time $t$ \n", |
| 43 | + "\n", |
38 | 44 | " \n",
|
39 | 45 | "\n",
|
40 | 46 | "The robot has a speed sensor and a gyro sensor.\n",
|
|
50 | 56 | "The input and observation vector includes sensor noise.\n",
|
51 | 57 | "\n",
|
52 | 58 | "In the code, \"observation\" function generates the input and observation vector with noise [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L34-L50)\n",
|
53 |
| - "\n", |
54 |
| - "\n", |
55 |
| - "\n", |
56 |
| - "\n", |
57 |
| - "\n", |
58 | 59 | "\n"
|
59 | 60 | ]
|
60 | 61 | },
|
|
75 | 76 | "\n",
|
76 | 77 | "So, the motion model is\n",
|
77 | 78 | "\n",
|
78 |
| - "$$\\textbf{x}_{t+1} = F\\textbf{x}+B\\textbf{u}$$\n", |
| 79 | + "$$\\textbf{x}_{t+1} = F\\textbf{x}_t+B\\textbf{u}_t$$\n", |
79 | 80 | "\n",
|
80 | 81 | "where\n",
|
81 | 82 | "\n",
|
|
103 | 104 | "\n",
|
104 | 105 | "This is implemented at [code](https://github.com/AtsushiSakai/PythonRobotics/blob/916b4382de090de29f54538b356cef1c811aacce/Localization/extended_kalman_filter/extended_kalman_filter.py#L53-L67)\n",
|
105 | 106 | "\n",
|
| 107 | + "Its Javaobian matrix is\n", |
| 108 | + "\n", |
| 109 | + "$\\begin{equation*}\n", |
| 110 | + "J_F=\n", |
| 111 | + "\\begin{bmatrix}\n", |
| 112 | + "\\frac{dx}{dx}& \\frac{dx}{dy} & \\frac{dx}{d\\phi} & \\frac{dx}{dv}\\\\\n", |
| 113 | + "\\frac{dy}{dx}& \\frac{dy}{dy} & \\frac{dy}{d\\phi} & \\frac{dy}{dv}\\\\\n", |
| 114 | + "\\frac{d\\phi}{dx}& \\frac{d\\phi}{dy} & \\frac{d\\phi}{d\\phi} & \\frac{d\\phi}{dv}\\\\\n", |
| 115 | + "\\frac{dv}{dx}& \\frac{dv}{dy} & \\frac{dv}{d\\phi} & \\frac{dv}{dv}\\\\\n", |
| 116 | + "\\end{bmatrix}\n", |
| 117 | + "=\n", |
| 118 | + "\\begin{bmatrix}\n", |
| 119 | + "1& 0 & -v sin(\\phi)dt & cos(\\phi)dt\\\\\n", |
| 120 | + "0 & 1 & v cos(\\phi)dt & sin(\\phi) dt\\\\\n", |
| 121 | + "0 & 0 & 1 & 0\\\\\n", |
| 122 | + "0 & 0 & 0 & 1\\\\\n", |
| 123 | + "\\end{bmatrix}\n", |
| 124 | + "\\end{equation*}$\n" |
| 125 | + ] |
| 126 | + }, |
| 127 | + { |
| 128 | + "cell_type": "markdown", |
| 129 | + "metadata": {}, |
| 130 | + "source": [ |
| 131 | + "# Observation Model\n", |
| 132 | + "\n", |
| 133 | + "The robot can get x-y position infomation from GPS.\n", |
| 134 | + "\n", |
| 135 | + "So GPS Observation model is\n", |
| 136 | + "\n", |
| 137 | + "$$\\textbf{z}_{t} = H\\textbf{x}_t$$\n", |
| 138 | + "\n", |
| 139 | + "where\n", |
| 140 | + "\n", |
| 141 | + "$\\begin{equation*}\n", |
| 142 | + "B=\n", |
| 143 | + "\\begin{bmatrix}\n", |
| 144 | + "1 & 0 & 0& 0\\\\\n", |
| 145 | + "0 & 1 & 0& 0\\\\\n", |
| 146 | + "\\end{bmatrix}\n", |
| 147 | + "\\end{equation*}$\n", |
| 148 | + "\n", |
| 149 | + "Its Jacobian matrix is\n", |
| 150 | + "\n", |
| 151 | + "$\\begin{equation*}\n", |
| 152 | + "J_H=\n", |
| 153 | + "\\begin{bmatrix}\n", |
| 154 | + "\\frac{dx}{dx}& \\frac{dx}{dy} & \\frac{dx}{d\\phi} & \\frac{dx}{dv}\\\\\n", |
| 155 | + "\\frac{dy}{dx}& \\frac{dy}{dy} & \\frac{dy}{d\\phi} & \\frac{dy}{dv}\\\\\n", |
| 156 | + "\\end{bmatrix}\n", |
| 157 | + "=\n", |
| 158 | + "\\begin{bmatrix}\n", |
| 159 | + "1& 0 & 0 & 0\\\\\n", |
| 160 | + "0 & 1 & 0 & 0\\\\\n", |
| 161 | + "\\end{bmatrix}\n", |
| 162 | + "\\end{equation*}$\n", |
106 | 163 | "\n"
|
107 | 164 | ]
|
108 | 165 | },
|
109 | 166 | {
|
110 | 167 | "cell_type": "markdown",
|
111 | 168 | "metadata": {},
|
112 | 169 | "source": [
|
113 |
| - "### Ref\n", |
| 170 | + "# Extented Kalman Filter\n", |
114 | 171 | "\n",
|
115 |
| - "- [PROBABILISTIC\\-ROBOTICS\\.ORG](http://www.probabilistic-robotics.org/)" |
| 172 | + "Localization process using Extendted Kalman Filter:EKF is\n", |
| 173 | + "\n", |
| 174 | + "=== Predict ===\n", |
| 175 | + "\n", |
| 176 | + "$x_{Pred} = Fx_t+Bu_t$\n", |
| 177 | + "\n", |
| 178 | + "$P_{Pred} = J_FP_t J_F^T + Q$\n", |
| 179 | + "\n", |
| 180 | + "=== Update ===\n", |
| 181 | + "\n", |
| 182 | + "$z_{Pred} = Hx_{Pred}$ \n", |
| 183 | + "\n", |
| 184 | + "$y = z - z_{Pred}$\n", |
| 185 | + "\n", |
| 186 | + "$ S = J_H P_{Pred}.J_H^T + R$\n", |
| 187 | + "\n", |
| 188 | + "$ K = P_{Pred}.J_H^T S^{-1}$\n", |
| 189 | + "\n", |
| 190 | + "\n", |
| 191 | + "$ x_{t+1} = x_{Pred} + Ky$\n", |
| 192 | + "\n", |
| 193 | + " $P_{t+1} = ( I - K J_H) P_{Pred} $" |
116 | 194 | ]
|
117 | 195 | },
|
118 | 196 | {
|
119 |
| - "cell_type": "code", |
120 |
| - "execution_count": null, |
| 197 | + "cell_type": "markdown", |
121 | 198 | "metadata": {},
|
122 |
| - "outputs": [], |
123 |
| - "source": [] |
| 199 | + "source": [ |
| 200 | + "### Ref\n", |
| 201 | + "\n", |
| 202 | + "- [PROBABILISTIC\\-ROBOTICS\\.ORG](http://www.probabilistic-robotics.org/)" |
| 203 | + ] |
124 | 204 | }
|
125 | 205 | ],
|
126 | 206 | "metadata": {
|
|
0 commit comments