pptacher / probabilistic_robotics Goto Github PK

solution of exercises of the book "probabilistic robotics"

License: MIT License

MATLAB 20.01% Python 8.42% Julia 7.42% C++ 37.60% Makefile 8.69% Shell 0.29% Dockerfile 0.85% TeX 16.62% C 0.11%

armadillo-library autonomous-vehicles bayesian-inference cpp fastslam google-cloud-platform intel-mkl-library julia kalman-filter lapack matlab navigation numpy particle-filter probabilistic-robotics robotics seif slam textbook-solutions victoria-park-dataset

probabilistic_robotics's People

Contributors

Stargazers

Watchers

Forkers

simonsroad forrest-z kelvinll willsiriusad botvgki charleshanch shyamalschandra goldenminerlmg tongsky723 litzar zhuangxiaopi gochinkushi challenzhou canling ortalby nhk035 zhangjun-xyz nuaalht alexmxb mxpfeng wanghuayou1028 jiayy2 ljf61 hongweisunny waywrong tutorgaming ewenwan chengyushi1 ottffive jason-zhangyp immaturehaan jason52 jastion 729630037 lidekanfa pankhurivanjani caoyifeng001 fly-duck wunl armendlee ngthanhtin lilcommieblyat wxb4733 cdsjatin zengphilo fred159 chenynan gary-robotics ning-yin charlesao fengbin311 stoneman404 autoperson fighterxi ranglang ehoxha91 hel4859 aust-hansen qingsimon jihyunyoon ronaldponguillo huangjianxjtu waltzrobotics dingziming handsomeboy sweetquiet zorosmith linye9 sutn konanrobot luoshuifish jyhjana cyy1991 jordanyang turtlezhong ziyu-deep microshy wbnature johnjin404 wjw25 chenbaifan afcarl weeken1029 maxtianyy ecustwuwenchao jayleelwj kuanqicai wh-0501 punith112 yt100323 chaof94 xiaodicn flyaudio xfyecn liousvious whydaydayup baikeshen cyc790 1122333-tv keystargchy

probabilistic_robotics's Issues

Where is the mat file in SEIF.m in the matlab code in Chapter 12?

I can't find the mat file in lines 9 and 10 of SEIF.m as far as I can find, so please add the mat file.

Chapter 3

Sorry, my calculation mistake.

What if the covariance matrix is singular?

In Ch.3 assignment 1.3， at t=1, the covariance matrix [0.25, 0.5; 0.5, 1.0] is a singular matrix. When we want to calculate its gaussian probabilistic, we need the matrix to be invertible. How can we deal with this situations.

ch3 ex_1.1 find a mistake about X_t

https://github.com/pptacher/probabilistic_robotics/blob/d31a9f1a3f5e500bd85d98d7fb4fb5743d1222ad/ch3_gaussian_filters/ch3_gaussian_filters.pdf

Question 1.1, 1.2

formula of the kalman filter should be:

$$ x_t = A_t x_{t-1} + B_t u_t + \epsilon_t $$

but not:

$$ x_t = A_t x_{t-1} + B_t u_{t-1} + \epsilon_t $$

which mean control variant $u_{t-1}$ on the last timestamp can't effect current state.

However, the distance (d), speed (v), acceleration (a) formula is:

$$ d_t = d_{t-1} + \Delta t \times v_{t-1} + \frac{1}{2} \Delta t^2 \times a_{t-1} $$

where acceleration ($a_{t-1}$) is from the last timestamp.

which means we need the acceleration from the last timestamp to update current distance and speed.

So we should make the acceleration into the consideration of state $X_t$.

And $X_t = [x_t, \dot{x}_t, \ddot{x}_t]^T$

Why the pdf of ch7 is so large?

the pdf file of other chapters are only 400kB, but for ch7 is more than 40MB, this make my Adobe Reader very slow. Is there any problem?

typo on ch2 4(b) page 11

the squre is not in the right place, and it should be like this:
wrong:

$$ \begin{align}-\frac{(z-x)^2}{2\sigma^2_{GPS}}-\frac{(x-x_{init})^2}{18\sigma^2_{GPS}} &= -\frac{9(z-x)^2+(x-x_{init})^2}{18\sigma^2_{GPS}} \\ &= -\frac{9(z^2-2zx+x^2)+(x^2-2xx_{init}+x_{init}^2)}{18\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}+10x^2-2xx_{init}-18zx}{18\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}+10x^2-2(x_{init}+9z)x}{18\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}-\frac{(x_{init}+9z)^2}{10}+10(x^2-2x\frac{x_{init}+9z}{10}+(\frac{x_{init}+9z}{10})^2)}{18\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}-\frac{(x_{init}+9z)^2}{10}}{18\sigma^2_{GPS}} -\frac{(x^2-2x\frac{x_{init}+9z}{10}+(\frac{x_{init}+9z}{10})^2)}{18/10\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}-\frac{(x_{init}+9z)^2}{10}}{18\sigma^2_{GPS}} -\frac{x-\frac{(x_{init}+9z)^2}{10}}{9/5\sigma^2_{GPS}} \\ \end{align} $$

right:

$$ \begin{align}-\frac{(z-x)^2}{2\sigma^2_{GPS}}-\frac{(x-x_{init})^2}{18\sigma^2_{GPS}} &= -\frac{9(z-x)^2+(x-x_{init})^2}{18\sigma^2_{GPS}} \\ &= -\frac{9(z^2-2zx+x^2)+(x^2-2xx_{init}+x_{init}^2)}{18\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}+10x^2-2xx_{init}-18zx}{18\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}+10x^2-2(x_{init}+9z)x}{18\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}-\frac{(x_{init}+9z)^2}{10}+10(x^2-2x\frac{x_{init}+9z}{10}+(\frac{x_{init}+9z}{10})^2)}{18\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}-\frac{(x_{init}+9z)^2}{10}}{18\sigma^2_{GPS}} -\frac{(x^2-2x\frac{x_{init}+9z}{10}+(\frac{x_{init}+9z}{10})^2)}{18/10\sigma^2_{GPS}} \\ &= -\frac{{9z^2+x_{init}^2}-\frac{(x_{init}+9z)^2}{10}}{18\sigma^2_{GPS}} -\frac{(x-\frac{x_{init}+9z}{10})^2}{9/5\sigma^2_{GPS}} \\ \end{align} $$

Chapter2 Exercise2.2&2.3 answer

Hello, I'm gonna ask a question about answers of Exercise2.2&2.3 of Chapter2, because in the PDF you didn't write any word in it. It is blank now.
Are exercise2.2&2.3 needed to solve by coding? If it does, could you give the answer code for me or share to everyone?

Thanks!

Ch.2 Exercise 3.1

Have you mixed up the transition-matrix for day 3-4? Although, day 5 is correct anyway because day 4 eliminates the error.

E.g.: My approx results for P(X_3=i | z_{2:3}) = (0.599, 0.282, 0.0)

Thx for your great work!

query about the code

Thanks for your sharing. I want to known that whether there are some python code resource about probabilitistic robotics especially in the localization. I will appreciate it if you can tell any message about this topics.

Thanks a lot!
Best Regards,
Wireless Sensor Networks, School of Computer Science, Sichuan University.

Wang Wei(王伟)， |mobile: +86-159-0810-6107 | email: [email protected]，[email protected] |website: https://oneway3124.github.io

problem 1 on chapter 2

I wanna know that why
P(X0=1) = p(1-p)
rather than
P(X0=1) = 1-p

Discussion on solution for problem 2.1 in chapter 6

Dear @pptacher
Thank you for your solutions which help me a lot. However I still have some concern about the solution for problem 2.1 in chapter 6.
As we know, the idea coordinates of the mark image in the camera plan is shown as vector $\begin{bmatrix} x_{i} \\ y_{i} \\ \theta _{i} \end{bmatrix}$ , while the real coordinates can be shown as vector $\begin{bmatrix} \widehat{x_{i}} \\ \widehat{y_{i}} \\ \widehat{\theta _{i}} \end{bmatrix}$ . Since the noise in measurement $\xi\sim N([0\, 0\, 0],\Sigma )$ , then
$\begin{bmatrix} \widehat{x_{i}} \\ \widehat{y_{i}} \\ \widehat{\theta _{i}} \end{bmatrix}= \begin{bmatrix} x_{i} \\ y_{i} \\ \theta _{i} \end{bmatrix}+\xi$

So, according to the solution of problem 1.1, we have
$\begin{bmatrix} x_{m} \\ y_{m} \\ \theta _{m} \end{bmatrix} = \begin{bmatrix} cos\theta\; -sin\theta \;\;0\\ sin\theta\;cos\theta\;\;0 \\ 0\; \; \; \; \; \; \;0\; \; \; \; \; \; 1 \end{bmatrix}\begin{bmatrix} \frac{h}{f}\; 0 \;0\\ 0\;\frac{h}{f}\; 0 \\ 0 \;\;0\;\;1 \end{bmatrix}( \begin{bmatrix} \widehat{x_{i}} \\ \widehat{y_{i}} \\ \widehat{{\theta _{i}}} \end{bmatrix}) +\begin{bmatrix} x_{r} \\ y_{r} \\ \theta _{r} \end{bmatrix}\\= \begin{bmatrix} cos\theta\; -sin\theta \;\;0\\ sin\theta\;cos\theta\;\;0 \\ 0\; \; \; \; \; \; \;0\; \; \; \; \; \; 1 \end{bmatrix}\begin{bmatrix} \frac{h}{f}\; 0 \;0\\ 0\;\frac{h}{f}\; 0 \\ 0 \;\;0\;\;1 \end{bmatrix}( \begin{bmatrix} {x_{i}} \\ {y_{i}} \\ {\theta _{i}} \end{bmatrix}+\xi) +\begin{bmatrix} x_{r} \\ y_{r} \\ \theta _{r} \end{bmatrix}$
Also it means that
$\begin{bmatrix} x_{m} \\ y_{m} \\ \theta _{m} \end{bmatrix} = \begin{bmatrix} cos\theta\; -sin\theta \;\;0\\ sin\theta\;cos\theta\;\;0 \\ 0\; \; \; \; \; \; \;0\; \; \; \; \; \; 1 \end{bmatrix}\begin{bmatrix} \frac{h}{f}\; 0 \;0\\ 0\;\frac{h}{f}\; 0 \\ 0 \;\;0\;\;1 \end{bmatrix}\xi + \begin{bmatrix} cos\theta\; -sin\theta \;\;0\\ sin\theta\;cos\theta\;\;0 \\ 0\; \; \; \; \; \; \;0\; \; \; \; \; \; 1 \end{bmatrix}\begin{bmatrix} \frac{h}{f}\; 0 \;0\\ 0\;\frac{h}{f}\; 0 \\ 0 \;\;0\;\;1 \end{bmatrix} \begin{bmatrix} {x_{i}} \\ {y_{i}} \\ {\theta _{i}} \end{bmatrix}+\begin{bmatrix} x_{r} \\ y_{r} \\ \theta _{r} \end{bmatrix}$

Considern that in problem 2.1 $\begin{bmatrix} x_{m} \\ y_{m} \\ \theta _{m} \end{bmatrix}\sim N(\mu _{m},\Sigma _{m})$ , then
$\Sigma _{m}= \begin{bmatrix} cos\theta\; -sin\theta \;\;0\\ sin\theta\;cos\theta\;\;0 \\ 0\; \; \; \; \; \; \;0\; \; \; \; \; \; 1 \end{bmatrix}\begin{bmatrix} \frac{h}{f}\; 0 \;0\\ 0\;\frac{h}{f}\; 0 \\ 0 \;\;0\;\;1 \end{bmatrix}\Sigma( \begin{bmatrix} cos\theta\; -sin\theta \;\;0\\ sin\theta\;cos\theta\;\;0 \\ 0\; \; \; \; \; \; \;0\; \; \; \; \; \; 1 \end{bmatrix}\begin{bmatrix} \frac{h}{f}\; 0 \;0\\ 0\;\frac{h}{f}\; 0 \\ 0 \;\;0\;\;1 \end{bmatrix})^{T} \\= \begin{bmatrix} cos\theta\; -sin\theta \;\;0\\ sin\theta\;cos\theta\;\;0 \\ 0\; \; \; \; \; \; \;0\; \; \; \; \; \; 1 \end{bmatrix}\begin{bmatrix} \frac{h}{f}\; 0 \;0\\ 0\;\frac{h}{f}\; 0 \\ 0 \;\;0\;\;1 \end{bmatrix}\begin{bmatrix} \sigma^{2}\;0\;0\\ 0\;\sigma^{2}\;0\\ 0\;\;0\;\;0\\ \end{bmatrix}( \begin{bmatrix} cos\theta\; -sin\theta \;\;0\\ sin\theta\;cos\theta\;\;0 \\ 0\; \; \; \; \; \; \;0\; \; \; \; \; \; 1 \end{bmatrix}\begin{bmatrix} \frac{h}{f}\; 0 \;0\\ 0\;\frac{h}{f}\; 0 \\ 0 \;\;0\;\;1 \end{bmatrix})^{T}$

Also it means that

$\Sigma _{m}=\begin{bmatrix} (\frac{h}{f}\sigma)^{2}\;0\;0\\ 0\;\;(\frac{h}{f}\sigma)^{2}\;0\\ 0\;\;0\;\;0\\ \end{bmatrix}$

rathern than $\begin{bmatrix} \sigma^{2}\;0\;0\\ 0\;\sigma^{2}\;0\\ 0\;\;0\;\;0\\ \end{bmatrix}$ as shown in the solution of problem 2.1
That is all and it is very appreciated to get your response in the future.

fastslam nodejs/c++ app is not working

When I clicked the link fastslam nodejs/c++ app, it is not working.