├── FUNDING.YML ├── LICENSE ├── README.md ├── ch10_simultaneous_localization_and_mapping └── ch10_simultaneous_localization_and_mapping.pdf ├── ch12_the_sparse_extended_information_filter ├── ch12_the_sparse_extended_information_filter.pdf ├── seif.jpg └── src │ ├── cpp │ ├── correspondence.cc │ ├── data │ │ ├── aa3_dr.h5 │ │ ├── aa3_lsr2.h5 │ │ ├── measurement.h5 │ │ └── timeLsr.bin │ ├── estimate.cc │ ├── linear_model.cc │ ├── main.cc │ ├── makefile │ ├── markov_blanket.cc │ ├── measurement.cc │ ├── motion.cc │ ├── seif.cc │ ├── seif.h │ ├── sparsification.cc │ └── to_UCN.sh │ ├── julia │ ├── LinearModel.jl │ ├── correspondence.jl │ ├── data │ │ ├── aa3_dr.h5 │ │ ├── aa3_lsr2.h5 │ │ └── z.h5 │ ├── estimate.jl │ ├── markov_blanket.jl │ ├── measurement.jl │ ├── motion.jl │ ├── seif.jl │ ├── seif_fct.jl │ ├── seif_launch.jl │ └── sparsification.jl │ ├── matlab │ ├── SEIF.m │ ├── SEIF_correspondence.m │ ├── SEIF_estimate.m │ ├── SEIF_measurement.m │ ├── SEIF_motion.m │ ├── SEIF_sparsification.m │ ├── equation_measurement.m │ ├── equation_motion.m │ ├── inverse_measurement.m │ ├── jacobian_measurement.m │ ├── jacobian_motion.m │ └── markov_blanket.m │ └── python │ ├── correspondence.py │ ├── equation_measurement.py │ ├── equation_motion.py │ ├── estimate.py │ ├── inverse_measurement.py │ ├── jacobian_measurement.py │ ├── jacobian_motion.py │ ├── markov_blanket.py │ ├── measurement.py │ ├── motion.py │ ├── seif.py │ └── sparsification.py ├── ch13_the_fastslam_algorithm └── src │ └── cpp │ ├── Dockerfile │ ├── README.md │ ├── btree.h │ ├── data │ ├── aa3_dr.h5 │ ├── aa3_lsr2.h5 │ ├── measurement.h5 │ └── timeLsr.bin │ ├── fastslam.cc │ ├── linear_model.cc │ ├── main.cc │ ├── makefile │ ├── model.h │ ├── particle.cc │ ├── particle.h │ └── to_UCN.sh ├── ch14_markov_decision_processes ├── ch14_markov_decision_processes.pdf ├── ch14_markov_decision_processes.tex ├── src │ ├── main.cc │ ├── makefile │ ├── markovdp.cc │ ├── markovdp.h │ ├── occupancy_map.h5 │ ├── to_UCN.sh │ └── v0.0 │ │ ├── main.cc │ │ ├── makefile │ │ ├── markovdp.cc │ │ ├── markovdp.h │ │ └── to_UCN.sh └── tikz1.tex ├── ch15_partially_observable_markov_decision_processes └── ch15_partially_observable_markov_decision_processes.pdf ├── ch2_recursive_state_estimation └── ch2_recursive_state_estimation.pdf ├── ch3_gaussian_filters ├── ch3_gaussian_filters.pdf ├── data.csv ├── data1.csv ├── data2.csv ├── data3.csv └── simmov.m ├── ch4_nonparametric_filters ├── ch4_nonparametric_filters.pdf ├── detect_circle.m ├── epankde.m ├── histogram_filter_1.m ├── histogram_filter_2.m ├── particle_filter_1.m ├── particle_filter_2.m ├── plotdistrib3.m └── simmov.m ├── ch5_robot_motion ├── bicyclespl.m └── ch5_robot_motion.pdf ├── ch6_robot_perception ├── ch6_robot_perception.pdf └── robotposespl.m ├── ch7_mrl_markov_and_gaussian ├── ch7_mrl_markov_and_gaussian.pdf ├── ekf.m └── simulation.m └── ch8_mrl_grid_and_monte_carlo ├── KLdivergence.m ├── ch8_mrl_grid_and_monte_carlo.pdf ├── histogram_filter_3.m ├── plotdistrib3.m └── post_resampling_dist.m /FUNDING.YML: -------------------------------------------------------------------------------- 1 | # repo: your_github_handle/project_name 2 | # filename: FUNDING.yml 3 | 4 | github: pptacher 5 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2018, Pierre-Paul TACHER. 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # probabilistic_robotics 2 | I am working on detailed solutions of exercises of the book "probabilistic robotics". This is a work in progress, any helpful feedback is welcomed. 3 | 4 | I also deployed the fastslam nodejs/c++ app on google cloud [here](http://35.242.140.13:8080) (server running from 0000 to 0800 UTC). 5 | 6 | ![alt text](https://github.com/pptacher/probabilistic_robotics/blob/master/ch12_the_sparse_extended_information_filter/seif.jpg) 7 | ***SEIF** algorithm running on Victoria Park dataset* 8 | 9 | 10 | ## references 11 | 12 | - *Probabilistic robotics*, *MIT press*, Sebastian Thrun, Wolfram Burgard and Dieter Fox, [Probabilistic Robotics](https://mitpress.mit.edu/books/probabilistic-robotics) 13 | 14 | - *Victoria Park dataset*, The University of Sidney, Eduardo Nebot, [Victoria Park dataset](http://www-personal.acfr.usyd.edu.au/nebot/victoria_park.htm) 15 | -------------------------------------------------------------------------------- /ch10_simultaneous_localization_and_mapping/ch10_simultaneous_localization_and_mapping.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch10_simultaneous_localization_and_mapping/ch10_simultaneous_localization_and_mapping.pdf -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/ch12_the_sparse_extended_information_filter.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch12_the_sparse_extended_information_filter/ch12_the_sparse_extended_information_filter.pdf -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/seif.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch12_the_sparse_extended_information_filter/seif.jpg -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/correspondence.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "seif.h" 3 | 4 | const static double π = 3.141592653589793238463; 5 | const static double c1 = 1.39; 6 | const static double c2 = 5.99; 7 | 8 | SpMat f_xm(uint,uint); 9 | double measure(double); 10 | 11 | std::vector correspondence(mat& z, std::vector& m0, vec& μ, vec& ξ, mat& Ω, SpMat& Λ){ 12 | uint k = size(z,1); 13 | 14 | if (k == 0) { 15 | return {}; 16 | } 17 | 18 | uint n = (μ.n_elem-3)/2; 19 | mat qnoise = diagmat(vec({5.0,0.02})); 20 | Col c(k); 21 | uvec newl; 22 | uvec new1; 23 | 24 | if (n > 0) { 25 | uint a(0); 26 | uint m(0); 27 | std::vector lst; 28 | 29 | for(auto iter=μ.begin()+3; iter!=μ.end(); iter+=2){ 30 | m++; 31 | float t1 = *iter-μ(0); 32 | float t2 = *(iter+1)-μ(1); 33 | if ( std::abs(t1) < 75 && std::abs(t2) < 75 34 | && t1*std::cos(μ(2)) + t2*std::sin(μ(2)) > -5 ) { 35 | ++a; 36 | lst.push_back(m); 37 | } 38 | } 39 | 40 | mat zh = equation_measurement(μ,lst); 41 | cube jcb = jacobian_measurement(μ,lst); 42 | mat q = zeros(2*a,2); 43 | uint i(0); 44 | 45 | for (auto iter=lst.begin(); iter!=lst.end(); ++iter,++i) { 46 | std::vector mkv = markov_blanket(*iter,m0,Λ); 47 | uvec indx = conv_to::from(mkv); 48 | 49 | uvec ind = zeros(1+2*indx.n_elem); 50 | ind(span(0,2)) = uvec({0,1,2}); 51 | for(uint ii=1; ii < indx.n_elem; ++ii){ 52 | ind(span(2*ii+1,2*ii+2)) = uvec({2*indx(ii)+1 ,2*indx(ii)+2}); 53 | } 54 | uvec ind1 = find(ind(span(3,ind.n_elem-1)) == 2**iter+1,1); 55 | SpMat f = f_xm(ind1(0),ind.size()); 56 | 57 | mat j = jcb.slice(i); 58 | superlu_opts opts; 59 | 60 | //opts.allow_ugly = true; 61 | //opts.symmetric = true; 62 | //opts.refine = superlu_opts::REF_NONE; 63 | //mat s(j*f*spsolve(sp_mat(symmatu(Ω(ind,ind))),f.t()*j.t(),"superlu",opts)+qnoise); 64 | //mat s(j*f*solve(symmatu(Ω(ind,ind)),f.t()*j.t(),solve_opts::fast)+qnoise); 65 | 66 | mat Ωii(trimatu(Ω(ind,ind))); 67 | mat jf(f.t()*j.t()); 68 | 69 | double *Ωii_mem = Ωii.memptr(); 70 | double *jf_mem = jf.memptr(); 71 | 72 | MKL_INT info; 73 | info = LAPACKE_dposv(LAPACK_COL_MAJOR,'U',1+2*indx.n_elem,2, 74 | Ωii_mem,1+2*indx.n_elem, 75 | jf_mem,ind.size()); 76 | if (info > 0) { 77 | exit(1); 78 | } 79 | 80 | //mat sol(jf_mem,1+2*indx.n_elem,2,false,true); 81 | mat s(j*f*jf+qnoise); 82 | 83 | vec eigval; 84 | mat eigvec; 85 | eig_sym(eigval,eigvec,s); 86 | eigval.for_each( [](mat::elem_type& val){ val = std::sqrt(1/val); }); 87 | 88 | q(span(2*i,2*i+1),span::all) = diagmat(eigval)*eigvec.t(); 89 | } 90 | 91 | mat f = zeros(2*a,k); 92 | for (size_t j = 0; j < a; j++) { 93 | f(span(2*j,2*j+1),span::all) = z.each_col() - zh.col(j); 94 | f.row(2*j+1).for_each( [](mat::elem_type& val){ val = measure(val); } ); 95 | } 96 | 97 | std::vector m1(m0); 98 | std::sort(m1.begin(),m1.end()); 99 | std::vector ia; //sorted indices of matched elements. 100 | std::vector cc; 101 | set_intersection_indx(lst.begin(), lst.end(), 102 | m1.begin(), m1.end(), 103 | std::back_inserter(cc), 104 | std::back_inserter(ia)); 105 | 106 | mat d1 = zeros(2*a,k); 107 | mat d2 = zeros(ia.size(),k); 108 | int l(0); 109 | for(auto iter=ia.begin(); iter!=ia.end(); ++iter,++l){ 110 | int j = *iter; 111 | d1(span(2*j,2*j+1),span::all) = 112 | q(span(2*j,2*j+1),span::all)*f(span(2*j,2*j+1),span::all); 113 | d2.row(l) = square(d1.row(2*j)) + square(d1.row(2*j+1)); 114 | } 115 | 116 | urowvec co(k); 117 | rowvec mins(k); 118 | for (size_t j = 0; j < k; j++) { 119 | co(j) = d2.col(j).index_min(); 120 | mins(j) = d2(co(j),j); 121 | } 122 | 123 | newl = find( mins > c1); 124 | new1 = find( mins(newl) < c2); 125 | 126 | if (new1.n_elem) { 127 | urowvec coo = zeros(new1.n_elem); 128 | for (size_t v = 0; v < new1.n_elem; v++) { 129 | coo(v) = co(newl(new1(v))); 130 | } 131 | urowvec cooo = unique(coo); 132 | for (int j = cooo.n_elem-1; j >= 0; --j) { 133 | ia.erase(ia.begin()+ cooo(j)); 134 | } 135 | } 136 | 137 | Col tmp(cc); 138 | c = tmp(co); 139 | 140 | if ((newl.n_elem > 0 && cc.size() < a) || cc.empty()){ 141 | if (cc.empty()) { 142 | newl = regspace(0,k-1); 143 | } 144 | std::vector tmpib(0); 145 | auto it2 = ia.begin(); 146 | for (size_t j = 0; 147 | j < a; ++j) { 148 | if (it2 == ia.end() || *it2 != j ) { 149 | tmpib.push_back(j); 150 | } 151 | else { 152 | ++it2; 153 | } 154 | } 155 | 156 | Col ib(tmpib); 157 | 158 | l = 0; 159 | mat d3 = zeros(tmpib.size(),newl.n_elem); 160 | for(auto iter=tmpib.begin(); iter!=tmpib.end(); ++iter,++l){ 161 | uint j = *iter; 162 | d1(uvec({2*j,2*j+1}),newl) = 163 | q(span(2*j,2*j+1),span::all)*f(uvec({2*j,2*j+1}),newl); 164 | d3.row(l) = square(d1(uvec({2*j}),newl)) + square(d1(uvec({2*j+1}),newl)); 165 | } 166 | 167 | if (!tmpib.empty()) { 168 | Col co1(newl.n_elem); 169 | rowvec mins1(newl.n_elem); 170 | for (size_t j = 0; j < newl.n_elem; j++) { 171 | co1(j) = d3.col(j).index_min(); 172 | c(newl(j)) = lst[ib(co1(j))]; 173 | mins1(j) = d3(co1(j),j); 174 | } 175 | 176 | new1 = {}; 177 | newl = newl( find(mins1 > c2) ); 178 | } 179 | } 180 | } // if (n>0) 181 | else { 182 | newl = regspace(0,k-1); 183 | new1 = {}; 184 | } 185 | if (new1.n_elem > 0) { 186 | uvec newtmp(newl.n_elem-new1.n_elem); 187 | uint j(0); 188 | uint b(0); 189 | for(size_t i = 0; i < newl.n_elem; ++i){ 190 | if (j < new1.n_elem && i == new1(j)) { 191 | ++j; 192 | } 193 | else { 194 | newtmp(b++) = newl(i); 195 | } 196 | } 197 | newl = newtmp; 198 | } 199 | if (newl.n_elem > 0) { 200 | uint j(n); 201 | for(size_t i = 0; i < newl.n_elem; ++i){ 202 | c(newl(i)) = ++j; 203 | } 204 | } 205 | std::vector corresp(k); 206 | for(size_t i = 0; i < k; ++i){ 207 | corresp[i] = c(i); 208 | } 209 | return corresp; 210 | 211 | } 212 | 213 | SpMat f_xm(uint p,uint n){ 214 | umat indx = {{0,1,2,3,4},{0,1,2,3+p,4+p}}; 215 | return SpMat(indx,ones>(5),5,n); 216 | } 217 | 218 | double measure(double θ){ 219 | double tmp = std::fmod(θ,2*π); 220 | return tmp - ((int) (tmp/π))*2*π; 221 | } 222 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/data/aa3_dr.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch12_the_sparse_extended_information_filter/src/cpp/data/aa3_dr.h5 -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/data/aa3_lsr2.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch12_the_sparse_extended_information_filter/src/cpp/data/aa3_lsr2.h5 -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/data/measurement.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch12_the_sparse_extended_information_filter/src/cpp/data/measurement.h5 -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/data/timeLsr.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch12_the_sparse_extended_information_filter/src/cpp/data/timeLsr.bin -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/estimate.cc: -------------------------------------------------------------------------------- 1 | #include "seif.h" 2 | 3 | void estimate(std::vector &m0, vec &μ, vec &ξ, mat &Ω){ 4 | uint n = (μ.n_elem-3)/2; 5 | 6 | if (n < 100) { 7 | μ = solve(Ω,ξ); 8 | } 9 | else { 10 | for (auto iter = m0.begin(); iter != m0.end(); ++iter) { 11 | 12 | auto indx = span(2**iter+1,2**iter+2); 13 | μ(indx) = solve(Ω(indx,indx),ξ(indx)-Ω(indx,span::all)*μ+Ω(indx,indx)*μ(indx)); 14 | 15 | /*mat a(trimatu(Ω(indx,indx))); 16 | mat b(ξ(indx)-Ω(indx,span::all)*μ+Ω(indx,indx)*μ(indx)); 17 | 18 | double *a_mem = a.memptr(); 19 | double *b_mem = b.memptr(); 20 | 21 | MKL_INT info; 22 | info = LAPACKE_dposv(LAPACK_COL_MAJOR,'U',2,1, 23 | a_mem,2, 24 | b_mem,2); 25 | if (info > 0) { 26 | exit(1); 27 | } 28 | μ(indx) = mat(b_mem,2,1,true);*/ 29 | } 30 | μ(span(0,2)) = solve(Ω(span(0,2),span(0,2)),ξ(span(0,2))-Ω(span(0,2),span::all)*μ+Ω(span(0,2),span(0,2))*μ(span(0,2))); 31 | } 32 | } 33 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/linear_model.cc: -------------------------------------------------------------------------------- 1 | #include "seif.h" 2 | 3 | const static double π = 3.141592653589793238463; 4 | const static float h = 0.74; 5 | const static float l = 2.83; 6 | const static float a = 0.95 + l; 7 | const static float b = 0.50; 8 | 9 | mat jacobian_motion(float θ, float v, float α, float dt){ 10 | return {{1, 0, -dt*(v*std::sin(θ)+v/l*std::tan(α)*(a*std::cos(θ)-b*std::sin(θ)))}, 11 | {0, 1, dt*(v*std::cos(θ)-v/l*std::tan(α)*(a*std::sin(θ)+b*std::cos(θ)))}, 12 | {0, 0, 1}}; 13 | } 14 | 15 | vec equation_motion(float θ, float v1, float α, float dt){ 16 | float v = v1/(1-std::tan(α)*h/l); 17 | return {dt*(v*std::cos(θ)-v/l*std::tan(α)*(a*std::sin(θ)+b*std::cos(θ))), 18 | dt*(v*std::sin(θ)+v/l*std::tan(α)*(a*std::cos(θ)-b*std::sin(θ))), 19 | dt*v/l*std::tan(α)}; 20 | } 21 | 22 | cube jacobian_measurement(vec μ, std::vector indx){ 23 | //pb converting Col to uvec = Col 24 | uint n = indx.size(); 25 | uvec indx1(n), indx2(n); 26 | uint i(0); 27 | for (auto it=indx.begin(); it != indx.end(); ++it, ++i){ 28 | indx1(i) = 2**it+1; 29 | indx2(i) = 2**it+2; 30 | } 31 | vec x = μ(indx1); 32 | vec y = μ(indx2); 33 | 34 | mat δ = mat(join_horiz(x,y)).each_row()-μ(span(0,1)).t(); 35 | vec q = sum(square(δ),1); 36 | 37 | mat jcb1 = mat({{1,0}, 38 | {0,-1}, 39 | {0, 1}, 40 | {1,0}})*δ.t(); 41 | cube jcb = zeros(10,n,1); 42 | jcb.slice(0) = join_vert(join_vert(join_vert(-jcb1,zeros(n)),-ones(n)),jcb1); 43 | jcb.slice(0).each_row(uvec({0,2,4,6,8})) %= rowvec(q.t()).for_each( [](mat::elem_type& val){ val = std::sqrt(1/val); }); 44 | jcb.slice(0).each_row(uvec({1,3,7,9})) %= rowvec(q.t()).for_each( [](mat::elem_type& val){ val = 1/val; }); 45 | 46 | return reshape(jcb,2,5,n); 47 | } 48 | 49 | mat equation_measurement(vec μ, std::vector indx){ 50 | uint n = indx.size(); 51 | uvec indx1(n), indx2(n); 52 | uint i(0); 53 | for (auto it=indx.begin(); it != indx.end(); ++it, ++i){ 54 | indx1(i) = 2**it+1; 55 | indx2(i) = 2**it+2; 56 | } 57 | vec x = μ(indx1); 58 | vec y = μ(indx2); 59 | mat δ = mat(join_horiz(x,y)).each_row()-μ(span(0,1)).t(); 60 | vec q = sqrt(sum(square(δ),1)); 61 | 62 | mat ϕ = mat(atan2(δ.col(1),δ.col(0))) - μ(2) + π/2; 63 | return mat(join_vert(q.t(),ϕ.t())); 64 | } 65 | 66 | mat inverse_measurement(vec μ, mat z){ 67 | rowvec c = cos(z.row(1)+μ(2)); 68 | rowvec s = sin(z.row(1)+μ(2)); 69 | 70 | return mat(join_vert(z.row(0)%s,-z.row(0)%c)).each_col() + μ(span(0,1)); 71 | } 72 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/main.cc: -------------------------------------------------------------------------------- 1 | #include "seif.h" 2 | 3 | int main(){ 4 | 5 | seif(); 6 | 7 | return 0; 8 | } 9 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/makefile: -------------------------------------------------------------------------------- 1 | help: 2 | @echo 3 | @echo "make lib <-- Compile lib." 4 | @echo "make build <-- Compile all." 5 | @echo "make clean <-- Clean up." 6 | @echo "make install <-- Compile and install" 7 | @echo "make dist <-- Create .tar.gz project file" 8 | @echo 9 | 10 | PROJECT_NAME = seif 11 | VERSION = 001 12 | 13 | ARCHIVE_DIR_NAME = $(PROJECT_NAME)-$(VERSION) 14 | ARCHIVE_NAME = $(ARCHIVE_DIR_NAME).tar.gz 15 | 16 | FILES_TO_BE_ARCHIVED = *.h *.cc Makefile 17 | 18 | INCLUDE_PATH = -I. -I/opt/intel/compilers_and_libraries/linux/mkl/include/ 19 | #problem with linking using gcc 20 | COMPILO = g++ 21 | CXXFLAGS = $(RELEASEFLAGS) -m64 -Wall -Wextra -Wshadow -Werror -pedantic $(INCLUDE_PATH) -Wno-deprecated-declarations -Wno-unused-variable -Wno-unused 22 | CXXFLAGS_SHARED = $(CXXFLAGS) -fPIC -shared 23 | LIBS = -L. -L/usr/lib/x86_64-linux-gnu/ -lseif -larmadillo -lhdf5_cpp 24 | 25 | INSTALL_PREFIX = ./LocalInstall 26 | 27 | INSTALL_BIN_DIR = $(INSTALL_PREFIX)/bin 28 | INSTALL_LIB_DIR = $(INSTALL_PREFIX)/lib 29 | INSTALL_INCLUDE_DIR = $(INSTALL_PREFIX)/include/seif 30 | 31 | INSTALL_DIRS = \ 32 | $(INSTALL_BIN_DIR) \ 33 | $(INSTALL_LIB_DIR) \ 34 | $(INSTALL_INCLUDE_DIR) 35 | 36 | ########################################################## 37 | #patch for gcc: pre process to convert UTF-8 encoding to Unicode code point. 38 | #ref: https://gcc.gnu.org/wiki/FAQ#What_is_the_status_of_adding_the_UTF-8_support_for_identifier_names_in_GCC.3F 39 | PROCESS_UTF8 = ./to_UCN.sh 40 | 41 | #SHELL := /bin/sh 42 | #CC := g++ 43 | #CXX := g++ 44 | 45 | export LIBRARY_PATH=/opt/intel/compilers_and_libraries/linux/mkl/lib/intel64_lin:/opt/intel/compilers_and_libraries/linux/lib/intel64 46 | #export MKL_VERBOSE=1 47 | 48 | COMPILER_OPTIONS := -m64 -Wall -Wextra -Wshadow -Werror -pedantic -Iinclude -Wno-unused-parameter -Wno-unused-variable -Wno-deprecated-declarations 49 | #INCLUDE_PATH = -I. 50 | 51 | CXX_DEFINES = -DSHADERS_DIR=\"/Users/TACHER/sparklepp/src/shaders\" -DUSE_GLEW 52 | 53 | #CFLAGS := -std=c11 $(COMPILER_OPTIONS) 54 | #CXXFLAGS := -std=c++14 -Weffc++ $(COMPILER_OPTIONS) $(INCLUDE_PATH) 55 | 56 | DEBUGFLAGS := -g -O0 -D _DEBUG 57 | RELEASEFLAGS := -O3 -D NDEBUG 58 | ########################################################## 59 | 60 | # Fabrication des .o (lib) 61 | 62 | seif.o : seif.cc 63 | $(PROCESS_UTF8) seif.cc 64 | $(COMPILO) -c -std=c++14 $(CXXFLAGS_SHARED) /tmp/seif.cc 65 | 66 | motion.o : motion.cc 67 | $(PROCESS_UTF8) motion.cc 68 | $(COMPILO) -c -std=c++14 $(CXXFLAGS_SHARED) /tmp/motion.cc 69 | 70 | estimate.o : estimate.cc 71 | $(PROCESS_UTF8) estimate.cc 72 | $(COMPILO) -c -std=c++14 $(CXXFLAGS_SHARED) /tmp/estimate.cc 73 | 74 | correspondence.o : correspondence.cc 75 | $(PROCESS_UTF8) correspondence.cc 76 | $(COMPILO) -c -std=c++14 $(CXXFLAGS_SHARED) /tmp/correspondence.cc 77 | 78 | markov_blanket.o : markov_blanket.cc 79 | $(PROCESS_UTF8) markov_blanket.cc 80 | $(COMPILO) -c -std=c++14 $(CXXFLAGS_SHARED) /tmp/markov_blanket.cc 81 | 82 | measurement.o : measurement.cc 83 | $(PROCESS_UTF8) measurement.cc 84 | $(COMPILO) -c -std=c++14 $(CXXFLAGS_SHARED) /tmp/measurement.cc 85 | 86 | sparsification.o : sparsification.cc 87 | $(PROCESS_UTF8) sparsification.cc 88 | $(COMPILO) -c -std=c++14 $(CXXFLAGS_SHARED) /tmp/sparsification.cc 89 | 90 | linear_model.o : linear_model.cc 91 | $(PROCESS_UTF8) linear_model.cc 92 | $(COMPILO) -c -std=c++14 $(CXXFLAGS_SHARED) /tmp/linear_model.cc 93 | 94 | # Fabrication des .o (hors lib) 95 | 96 | main.o : main.cc 97 | $(COMPILO) -c -std=c++14 $(CXXFLAGS) main.cc 98 | 99 | # Fabrication de la lib 100 | 101 | libseif.so : seif.o motion.o estimate.o correspondence.o measurement.o sparsification.o linear_model.o markov_blanket.o 102 | $(COMPILO) -o libseif.so -shared -larmadillo seif.o motion.o estimate.o correspondence.o measurement.o sparsification.o linear_model.o markov_blanket.o 103 | 104 | # Fabrication de l'exécutable 105 | 106 | seif.bin : main.o libseif.so 107 | $(COMPILO) -o seif.bin main.o $(LIBS) -lpthread -lmkl_core -lmkl_def -lmkl_intel_thread -lmkl_core -liomp5 -lm -ldl 108 | #-DARMA_DONT_USE_WRAPPER -lmkl_intel_thread -lmkl_intel_ilp64 109 | phony : 110 | echo $(LIBRARY_PATH) 111 | 112 | # Les cibles de l'aide 113 | 114 | lib : libseif.so 115 | 116 | build : seif.bin 117 | 118 | clean : 119 | @rm -f *~ *.o *.so *.bin 120 | @rm -rf $(ARCHIVE_DIR_NAME) $(ARCHIVE_NAME) 121 | @echo 122 | @echo "Cleaned up." 123 | 124 | install : seif.bin libseif.so 125 | @mkdir -p $(INSTALL_DIRS) 126 | @cp seif.bin $(INSTALL_BIN_DIR) 127 | @cp libseif.so $(INSTALL_LIB_DIR) 128 | @cp *.h $(INSTALL_INCLUDE_DIR) 129 | @echo "File installed in " $(INSTALL_BIN_DIR) $(INSTALL_LIB_DIR) $(INSTALL_INCLUDE_DIR) 130 | 131 | dist : 132 | @rm -rf $(ARCHIVE_DIR_NAME) $(ARCHIVE_NAME) 133 | @mkdir $(ARCHIVE_DIR_NAME) 134 | @cp $(FILES_TO_BE_ARCHIVED) $(ARCHIVE_DIR_NAME) 135 | @tar zcf $(ARCHIVE_NAME) $(ARCHIVE_DIR_NAME) 136 | @rm -rf $(ARCHIVE_DIR_NAME) 137 | @echo "File" $(ARCHIVE_NAME) " generated." 138 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/markov_blanket.cc: -------------------------------------------------------------------------------- 1 | #include "seif.h" 2 | 3 | std::vector bfs(uint, SpMat&); 4 | 5 | std::vector markov_blanket(uint n,std::vector& m0, SpMat& Λ){ 6 | uint sg = size(Λ,0); 7 | std::vector mb1(m0); 8 | uvec mb = find(Row(Λ.row(n)) != 0); 9 | 10 | std::vector mb2(mb.n_elem); 11 | uint j=0; 12 | if (mb(0) != 0) { 13 | mb2.push_back(0u); 14 | mb2[j++] = 0; 15 | } 16 | for(size_t i=0; i mb12, mb4; 21 | std::sort(mb1.begin(),mb1.end()); 22 | std::set_intersection(mb1.begin(),mb1.end(), 23 | mb2.begin(),mb2.end(), 24 | std::back_inserter(mb12)); 25 | if (mb12.empty()) { 26 | std::vector mb3; 27 | mb3 = bfs(n,Λ); 28 | if (!mb3.empty()) {//should be useless. 29 | std::sort(mb3.begin(),mb3.end()); 30 | std::set_union(mb3.begin(),mb3.end(), 31 | mb2.begin(),mb2.end(), 32 | std::back_inserter(mb4)); 33 | } 34 | } 35 | else { 36 | mb4 = mb2; 37 | } 38 | 39 | std::vector mb5; 40 | std::set_union(mb4.begin(),mb4.end(), 41 | mb1.begin(),mb1.end(), 42 | std::back_inserter(mb5)); 43 | 44 | return mb5; 45 | } 46 | 47 | std::vector bfs(uint n, SpMat& Λ){ 48 | uint sg = size(Λ,0); 49 | std::vector vistd(sg,-1); 50 | vistd[0] = 0; 51 | std::queue q; 52 | q.push(0); 53 | bool found(false); 54 | uint currt(0); 55 | 56 | while (!found && !q.empty()) { 57 | currt = q.front(); 58 | q.pop(); 59 | for (size_t i = 0; i < sg; i++) { 60 | if (i != currt && Λ.row(currt)(i) && vistd[i]==-1) { 61 | q.push(i); 62 | vistd[i] = currt; 63 | } 64 | } 65 | found = vistd[n]==static_cast(currt); 66 | } 67 | 68 | std::vector path = {0}; 69 | if (found) { 70 | path.push_back(currt); 71 | int prev = vistd[currt]; 72 | while (prev) { 73 | path.push_back(prev); 74 | prev = vistd[prev]; 75 | } 76 | } 77 | 78 | return path; 79 | } 80 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/measurement.cc: -------------------------------------------------------------------------------- 1 | #include "seif.h" 2 | 3 | const static double π = 3.141592653589793238463; 4 | 5 | double measure(double); 6 | 7 | void measurement(mat& z, std::vector& c, vec& μ, vec& ξ, mat& Ω, SpMat& Λ){ 8 | uint n = (μ.n_elem-3)/2; 9 | mat qnoise = diagmat(vec({5.0,0.02})); 10 | Col cc(c); 11 | int nnew = max(cc) - n; 12 | 13 | if (nnew > 0) { 14 | uint osize = ξ.n_elem; 15 | uint nsize = osize + 2*nnew; 16 | Ω.resize(nsize,nsize); 17 | ξ.resize(nsize); 18 | Λ.resize(n+1+nnew, n+1+nnew); 19 | μ.resize(nsize); 20 | mat tmp = inverse_measurement(μ(span(0,2)), z(uvec({0,1}), find(cc > n))); 21 | μ(span(osize,nsize-1)) = vectorise(tmp); 22 | } 23 | 24 | cube jcb = jacobian_measurement(μ,c); 25 | mat zhat = equation_measurement(μ,c); 26 | mat qnoiseinv = inv(qnoise); 27 | 28 | for (size_t i = 0; i < c.size(); i++) { 29 | uint j = c[i]; 30 | mat h = jcb.slice(i); 31 | vec dz = z.col(i)-zhat.col(i); 32 | dz(1) = measure(dz(1)); 33 | uvec indx = {0,1,2,2*j+1,2*j+2}; 34 | 35 | ξ(indx) += h.t()*qnoiseinv*(dz+h*μ(indx)); 36 | Ω(indx,indx) += h.t()*qnoiseinv*h; 37 | } 38 | 39 | Col ccc = unique(cc); 40 | umat edges = zeros(2,2*ccc.n_elem); 41 | uint i(0); 42 | for (size_t j=0; j adjacency(edges,ones>(2*ccc.n_elem),Λ.n_rows,Λ.n_cols); 48 | Λ += adjacency + adjacency.t(); 49 | 50 | } 51 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/motion.cc: -------------------------------------------------------------------------------- 1 | #include "seif.h" 2 | 3 | void motion(float v, float α, float dt, std::vector &m0, vec &μ, vec &ξ, mat &Ω, SpMat &Λ){ 4 | size_t n = ξ.n_elem; 5 | mat rnoise = diagmat(vec({8e-4,8e-4,1e-6})); 6 | 7 | mat jcb = jacobian_motion(μ(2),v,α,dt); 8 | 9 | mat ψ = inv(jcb) - eye(3,3); 10 | uvec indx = {0,1,2}; 11 | std::vector m1(m0); 12 | sort(m1.begin(),m1.end()); 13 | 14 | uvec ind(3+2*m1.size()); 15 | ind(uvec({0,1,2})) = uvec({0,1,2}); 16 | uint j(0); 17 | for (auto iter=m1.begin(); iter != m1.end(); ++iter, j+=2) { 18 | ind(3+j) = 2**iter+1; 19 | ind(4+j) = 2**iter+2; 20 | } 21 | mat Ω₁ = Ω(ind,uvec(indx)); 22 | mat ψ₁ = Ω₁*ψ; 23 | 24 | mat λ = zeros(n,n); 25 | λ(ind,indx) = ψ₁; 26 | λ(indx,ind) += ψ₁.t(); 27 | λ(indx,indx) += ψ.t()*Ω(indx,indx)*ψ; 28 | mat Φ = Ω + λ; 29 | 30 | mat Φₓ = Φ(indx,ind); 31 | mat κ = zeros(n,n); 32 | κ(ind,ind) = Φₓ.t()*inv(inv(rnoise)+Φ(indx,indx))*Φₓ; 33 | Ω += λ - κ; 34 | 35 | vec δ = equation_motion(μ(2),v,α,dt); 36 | 37 | mat lk = λ-κ; 38 | ξ(ind) += Ω(ind,indx)*δ + lk.rows(ind)*μ; 39 | μ(span(0,2)) += δ; 40 | 41 | if (!m0.empty()) { 42 | umat edges = zeros(2,m1.size()*m1.size()); 43 | uint i(0); 44 | for (auto iter=m1.begin(); iter != m1.end(); ++iter) { 45 | for (auto iter1 = m1.begin(); iter1 != m1.end(); ++iter1) { 46 | edges(0,i) = *iter; 47 | edges(1,i++) = *iter1; 48 | } 49 | } 50 | if (!edges.empty()) { 51 | SpMat adjacency(edges,ones>(edges.n_cols),Λ.n_rows,Λ.n_cols); 52 | Λ += adjacency; 53 | } 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/seif.cc: -------------------------------------------------------------------------------- 1 | #include "seif.h" 2 | 3 | void seif(){ 4 | std::string filename1 = "aa3_lsr2.h5"; 5 | std::string filename2 = "aa3_dr.h5"; 6 | std::string filename3 = "measurement.h5"; 7 | std::string filename4 = "timeLsr.bin"; 8 | std::string include_dir = "./data/"; 9 | 10 | std::string filename5 = include_dir + "poses.dat"; 11 | std::ofstream ostrm(filename5, std::ios::binary); 12 | 13 | arma::mat speed, steering, time, measurmt; 14 | uvec timelsr; 15 | speed.load(arma::hdf5_name(include_dir + filename2, "speed")); 16 | steering.load(arma::hdf5_name(include_dir + filename2, "steering")); 17 | time.load(arma::hdf5_name(include_dir + filename2, "time")); 18 | timelsr.load(include_dir + filename4); 19 | measurmt.load(arma::hdf5_name(include_dir + filename3, "zz")); 20 | 21 | vec μ = zeros(3); 22 | vec ξ = zeros(3); 23 | mat Ω = 10e4*eye(3,3); 24 | std::vector m0; 25 | SpMat Λ; 26 | 27 | static uint const n(20); 28 | static float const dt = 25e-3; 29 | 30 | uint stindex = 0; 31 | uvec jvec = find(timelsr >= time(stindex),1,"first"); 32 | uint llsr = timelsr.n_elem; 33 | uint j = jvec(0); 34 | 35 | //mat::fixed<5000,2> poses = zeros(5000,2); 36 | 37 | //uint max_threads = mkl_get_max_threads(); 38 | //mkl_set_num_threads(max_threads); 39 | 40 | std::chrono::steady_clock::time_point time_; 41 | std::chrono::steady_clock::time_point start_time_; 42 | std::chrono::steady_clock::time_point start_iter_; 43 | float deltatime_(0.0f); 44 | float deltatime_iter_(0.0f); 45 | start_time_ = std::chrono::steady_clock::now(); 46 | 47 | 48 | for (size_t i = stindex; i < time.n_elem; i++) { 49 | start_iter_ = std::chrono::steady_clock::now(); 50 | 51 | if (i > stindex && i%5000==0) { 52 | /*std::string filename = include_dir + "iter_" + std::to_string(i) + ".h5"; 53 | poses.save(hdf5_name(filename,"poses")); 54 | μ.save(hdf5_name(filename, "μ")); 55 | ξ.save(hdf5_name(filename, "ξ")); 56 | Ω.save(hdf5_name(filename, "Ω")); 57 | Col(m0).save(hdf5_name(filename, "m0")); 58 | Λ.save(include_dir + "Λ_iter_" + std::to_string(i) + ".bin");*/ 59 | 60 | /*std::string filename = include_dir + "iter_" + std::to_string(i) + ".dat"; 61 | poses.save(include_dir + "poses_" + std::to_string(i) + ".dat"); 62 | μ.save(include_dir + "μ_" + std::to_string(i) + ".dat"); 63 | ξ.save(include_dir + "ξ_" + std::to_string(i) + ".dat"); 64 | Ω.save(include_dir + "Ω_" + std::to_string(i) + ".dat"); 65 | Col(m0).save(include_dir + "m0_" + std::to_string(i) + ".dat"); 66 | Λ.save(include_dir + "Λ_" + std::to_string(i) + ".dat");*/ 67 | } 68 | 69 | motion(speed(i),steering(i),dt,m0,μ,ξ,Ω,Λ); 70 | 71 | estimate(m0,μ,ξ,Ω); 72 | 73 | std::vector m3; 74 | std::vector dest; 75 | while (j <= llsr && timelsr(j) < time(i)+dt) { 76 | mat z1 = measurmt(uvec({2*j,2*j+1}),find(measurmt(2*j,span::all) != 0)); 77 | if (!z1.empty()) { 78 | std::vector c; 79 | c = correspondence(z1,m0,μ,ξ,Ω,Λ); 80 | measurement(z1,c,μ,ξ,Ω,Λ); 81 | std::sort(c.begin(),c.end()); 82 | std::set_union(m3.begin(),m3.end(),c.begin(),c.end(),std::back_inserter(dest)); 83 | std::swap(m3,dest); 84 | } 85 | 86 | j++; 87 | } 88 | 89 | if (!m3.empty()) { 90 | 91 | std::vector m1, m4, m5, m6, m2(m0); 92 | 93 | std::sort(m2.begin(),m2.end()); 94 | std::set_difference(m2.begin(),m2.end(), 95 | m3.begin(),m3.end(), 96 | std::inserter(m4,m4.begin())); 97 | auto last = std::unique(m3.begin(),m3.end()); 98 | m4.insert(m4.end(),m3.begin(),last); 99 | m4.erase(m4.begin(),m4.begin()+std::max(0,int(m4.size()-n))); 100 | 101 | std::vector m8(m4); 102 | std::sort(m8.begin(),m8.end()); 103 | 104 | std::set_difference(m2.begin(),m2.end(), 105 | m8.begin(),m8.end(), 106 | std::inserter(m1,m1.begin())); 107 | 108 | std::set_difference(m3.begin(),last, 109 | m8.begin(),m8.end(), 110 | std::inserter(m5,m5.begin())); 111 | 112 | std::set_union(m1.begin(),m1.end(),m5.begin(),m5.end(),std::back_inserter(m6)); 113 | m0 = std::move(m4); 114 | if (!m6.empty()) { 115 | sparsification(m0,m6,μ,ξ,Ω,Λ); 116 | } 117 | } 118 | 119 | time_ = std::chrono::steady_clock::now(); 120 | deltatime_ = std::chrono::duration_cast>(time_-start_time_).count(); 121 | deltatime_iter_ = std::chrono::duration_cast>(time_-start_iter_).count(); 122 | std::cout << "iter: " << std::setw(10) << i << 123 | " iter time: " << std::setw(10) << deltatime_iter_ << 124 | " avg: " << std::setw(10) << deltatime_/(i+1) << 125 | '\r' ; 126 | 127 | ostrm << std::setw(15) << μ(0) << 128 | std::setw(15) << μ(1) << '\n'; 129 | 130 | } 131 | std::cout << '\n'; 132 | } 133 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/seif.h: -------------------------------------------------------------------------------- 1 | #define ARMA_USE_HDF5 2 | //#define ARMA_USE_SUPERLU 1 3 | #define ARMA_64BIT_WORD 4 | #define DARMA_DONT_USE_WRAPPER 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include "mkl_lapacke.h" 17 | //#include "mkl.h" 18 | 19 | ///////////////////////////////////////////////////////////////////// 20 | //gdb helpers 21 | template 22 | void print_matrix(Matrix matrix) { 23 | matrix.print(std::cout); 24 | } 25 | 26 | 27 | template void print_matrix(arma::mat matrix); 28 | template void print_matrix(arma::cx_mat matrix); 29 | template void print_matrix>(arma::Col); 30 | template void print_matrix(arma::uvec); 31 | template void print_matrix(arma::rowvec); 32 | template void print_matrix(arma::vec); 33 | ///////////////////////////////////////////////////////////////////// 34 | 35 | using namespace arma; 36 | 37 | void seif(void); 38 | void motion(float, float, float, std::vector&, vec&, vec&, mat&, SpMat&); 39 | void estimate(std::vector&, vec&, vec&, mat&); 40 | std::vector correspondence(mat&, std::vector&, vec&, vec&, mat&, SpMat&); 41 | void measurement(mat&, std::vector&, vec&, vec&, mat&, SpMat&); 42 | void sparsification(std::vector&, std::vector& ,vec&, vec&, mat&, SpMat&); 43 | 44 | std::vector markov_blanket(uint, std::vector&, SpMat&); 45 | 46 | mat jacobian_motion(float, float, float, float); 47 | vec equation_motion(float, float, float, float); 48 | cube jacobian_measurement(vec, std::vector); 49 | mat equation_measurement(vec, std::vector); 50 | mat inverse_measurement(vec, mat); 51 | 52 | template 53 | OutputIt1 set_intersection_indx(InputIt1 first1, InputIt1 last1, 54 | InputIt2 first2, InputIt2 last2, 55 | OutputIt1 d_first, OutputIt2 indx){ 56 | auto tmp = first1; 57 | while (first1 != last1 && first2 != last2) { 58 | if (*first1 < *first2) { 59 | ++first1; 60 | } else { 61 | if (!(*first2 < *first1)) { 62 | *indx++ = std::distance(tmp, first1); 63 | *d_first++ = *first1++; 64 | } 65 | ++first2; 66 | } 67 | } 68 | return d_first; 69 | } 70 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/sparsification.cc: -------------------------------------------------------------------------------- 1 | #include "seif.h" 2 | 3 | void sparsification(std::vector& m0, std::vector& m6, vec& μ, vec& ξ, mat& Ω, SpMat& Λ){ 4 | 5 | std::vector m7(m0); 6 | std::vector m8(m6); 7 | std::vector m9; 8 | 9 | std::sort(m7.begin(), m7.end()); 10 | std::sort(m8.begin(), m8.end()); 11 | std::set_union(m7.begin(), m7.end(), 12 | m8.begin(), m8.end(), 13 | std::back_inserter(m9)); 14 | 15 | for (auto it1 = m6.begin(); it1 != m6.end(); ++it1 ){ 16 | Λ(0,*it1) = 0; 17 | Λ(*it1,0) = 0; 18 | for (auto it2 = m9.begin(); it2 != m9.end(); ++it2){ 19 | if (*it1 != *it2) { 20 | Λ(*it1,*it2) = 1; 21 | Λ(*it2,*it1) = 1; 22 | } 23 | } 24 | } 25 | uint n0(2*m7.size()), n6(2*m8.size()); 26 | uvec indx0(n0), indx6(n6); 27 | uint i(0); 28 | for (auto it=m7.begin(); it != m7.end(); ++it,i+=2){ 29 | indx0(i) = 2**it+1; 30 | indx0(i+1) = 2**it+2; 31 | } 32 | i=0; 33 | for (auto it=m8.begin(); it != m8.end(); ++it, i+=2){ 34 | indx6(i) = 2**it+1; 35 | indx6(i+1) = 2**it+2; 36 | } 37 | 38 | MKL_INT info; 39 | 40 | mat Ω₁ = Ω(indx6, 41 | join_vert(join_vert(uvec({0,1,2}),indx0),indx6)); 42 | mat a1(trimatu(Ω(indx6,indx6))); 43 | mat Ω₁T(Ω₁.t()); 44 | 45 | double *a1_mem = a1.memptr(); 46 | double *b1_mem = Ω₁.memptr(); 47 | 48 | info = LAPACKE_dposv(LAPACK_COL_MAJOR,'U',n6,3+n0+n6, 49 | a1_mem,n6, 50 | b1_mem,n6); 51 | if (info > 0) { 52 | exit(1); 53 | } 54 | //mat sol1(b1_mem,n6,3+n0+n6,false,true); 55 | mat l1 = Ω₁T*Ω₁; 56 | //mat l1 = Ω₁.t()*solve(symmatu(Ω(indx6,indx6)),Ω₁,solve_opts::fast); 57 | 58 | mat Ω₂ = Ω(join_vert(uvec({0,1,2}),indx6), 59 | join_vert(join_vert(uvec({0,1,2}),indx0),indx6)); 60 | 61 | mat Ω₂T(Ω₂.t()); 62 | mat a2(trimatu(Ω(join_vert(uvec({0,1,2}),indx6),join_vert(uvec({0,1,2}),indx6)))); 63 | 64 | double *a2_mem = a2.memptr(); 65 | double *b2_mem = Ω₂.memptr(); 66 | 67 | info = LAPACKE_dposv(LAPACK_COL_MAJOR,'U',3+n6,3+n0+n6, 68 | a2_mem,3+n6, 69 | b2_mem,3+n6); 70 | if (info > 0) { 71 | exit(1); 72 | } 73 | //mat* sol2 = new mat(b2_mem,3+n6,3+n0+n6,false,true); 74 | mat l2 = Ω₂T*Ω₂; 75 | //delete sol2;*/ 76 | //mat l2 = Ω₂.t() * solve(symmatu(Ω(join_vert(uvec({0,1,2}),indx6),join_vert(uvec({0,1,2}),indx6))),Ω₂,solve_opts::fast); 77 | 78 | mat Ω₃ = Ω(span(0,2),span::all); 79 | mat Ω₃T(Ω₃.t()); 80 | mat a3(trimatu(Ω(span(0,2),span(0,2)))); 81 | 82 | double *a3_mem = a3.memptr(); 83 | double *b3_mem = Ω₃.memptr(); 84 | 85 | info = LAPACKE_dposv(LAPACK_COL_MAJOR,'U',3,Ω.n_rows, 86 | a3_mem,3, 87 | b3_mem,3); 88 | if (info > 0) { 89 | exit(1); 90 | } 91 | //mat sol3(b3_mem,3,Ω.n_rows,false,true); 92 | mat l3 = Ω₃T*Ω₃; 93 | 94 | //mat l3 =Ω₃.t() * solve(symmatu(Ω(span(0,2),span(0,2))),Ω₃,solve_opts::fast); 95 | 96 | mat Ω₄(Ω); 97 | Ω(join_vert(join_vert(uvec({0,1,2}),indx0),indx6), 98 | join_vert(join_vert(uvec({0,1,2}),indx0),indx6)) -= l1; 99 | Ω(join_vert(join_vert(uvec({0,1,2}),indx0),indx6), 100 | join_vert(join_vert(uvec({0,1,2}),indx0),indx6)) += l2; 101 | Ω -= l3; 102 | 103 | //correct rounding errors. 104 | Ω(uvec({0,1,2}),indx6).zeros(); 105 | Ω(indx6,uvec({0,1,2})).zeros(); 106 | 107 | ξ += (Ω-Ω₄)*μ; 108 | } 109 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/cpp/to_UCN.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | cat $1 | perl -pe 'BEGIN { binmode STDIN, ":utf8"; } s/(.)/ord($1) < 128 ? $1 : sprintf("\\U%08x", ord($1))/ge;' > /tmp/$1 3 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/LinearModel.jl: -------------------------------------------------------------------------------- 1 | module LinearModel 2 | 3 | export jacobian_motion, equation_motion, 4 | jacobian_measurement, equation_measurement, inverse_measurement 5 | 6 | H = 0.74 7 | L = 2.83 8 | a = 0.95 + L 9 | b = 0.50 10 | 11 | function jacobian_motion(θ,v,α,dt) 12 | [[1 0 -dt*(v*sin(θ)+v/L*tan(α)*(a*cos(θ)-b*sin(θ)))]; 13 | [0 1 dt*(v*cos(θ)-v/L*tan(α)*(a*sin(θ)+b*cos(θ)))]; 14 | [0 0 1]] 15 | end 16 | 17 | function equation_motion(θ,v,α,dt) 18 | v /= 1-tan(α)*H/L 19 | [dt*(v*cos(θ)-v/L*tan(α)*(a*sin(θ)+b*cos(θ))), 20 | dt*(v*sin(θ)+v/L*tan(α)*(a*cos(θ)-b*sin(θ))), 21 | dt*v/L*tan(α)] 22 | end 23 | 24 | function jacobian_measurement(μ, ind) 25 | n = length(ind) 26 | 27 | x = μ[2*ind .+ 2] 28 | y = μ[2*ind .+ 3] 29 | 30 | δ = [x y] .- μ[1:2]' 31 | q = sum(δ.^2,dims=2) 32 | 33 | J = [1 0;0 -1;0 1;1 0]*δ' 34 | J = [-J;zeros(1,n);-ones(1,n);J] 35 | J[1:2:end,:] .*= 1 ./ sqrt.(q') 36 | J[[2,4,8,10],:] .*= 1 ./ q' 37 | 38 | return reshape(J,(2,5,n)) 39 | end 40 | 41 | function equation_measurement(μ,ind) 42 | n = length(ind) 43 | 44 | x = μ[2*ind .+ 2] 45 | y = μ[2*ind .+ 3] 46 | 47 | δ = [x y] .- μ[1:2]' 48 | q = sqrt.(sum(δ.^2,dims=2)) 49 | 50 | [q atan.(δ[:,2],δ[:,1]).-μ[3].+π/2]' 51 | end 52 | 53 | function inverse_measurement(μ,z) 54 | c = cos.(μ[3] .+ z[2,:]) 55 | s = sin.(μ[3] .+ z[2,:]) 56 | 57 | return μ[1:2] .+ [(z[1,:].*s)'; (-z[1,:].*c)'] 58 | end 59 | 60 | end 61 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/correspondence.jl: -------------------------------------------------------------------------------- 1 | function correspondence(z,m₀,μ,Ω,graph) 2 | k = size(z,2) 3 | if k == 0 4 | return zeros(0) 5 | end 6 | n = (length(μ)-3)÷2 7 | qnoise = diagm( 0 => [5.0,0.02]) 8 | co = zeros(Int,k) 9 | 10 | if n > 0 11 | t = μ[4:end] 12 | t₁ = t[1:2:end] 13 | t₂ = t[2:2:end] 14 | lst = findall((abs.(t₁.-μ[1]) .≤ 75) .* 15 | (abs.(t₂.-μ[2]) .≤ 75) .* 16 | ((t₁.-μ[1])*cos(μ[3]) .+ (t₂.-μ[2])*sin(μ[3]) .≥ -5)) 17 | a = length(lst) 18 | jcb = jacobian_measurement(μ,lst) 19 | ẑ = reshape(equation_measurement(μ,lst),(2,1,a)) 20 | 21 | q = SharedArray{Float64,2}((2a,2)) 22 | 23 | @sync begin 24 | for p in procs(q) 25 | @async remotecall_wait(compute_cov_shared!,p, 26 | Ω,lst,m₀,graph,q,jcb,qnoise) 27 | end 28 | end 29 | 30 | f = reshape(permutedims(ẑ.-z,[1,3,2]),(2*a,k)) 31 | f[2:2:end,:] = measure.(f[2:2:end,:]) 32 | d₁ = zeros(2a,k) 33 | cc, ia = intersect_ind(lst,m₀,n) 34 | 35 | @inbounds for j ∈ ia 36 | d₁[2j-1:2j,:] = q[2j-1:2j,:]*f[2j-1:2j,:] 37 | end 38 | 39 | ind = [[2j-1,2j] for j ∈ ia] 40 | d₂ = d₁[vcat(ind...),:] 41 | d₂ .= d₂.^2 42 | d₃ = [d₂[i,j]+d₂[i+1,j] for i ∈ 1:2:2length(ia),j ∈ 1:k] 43 | 44 | co = argmin(d₃,dims=1) 45 | co = co[:] 46 | co = getindex.(co,1) 47 | m₁ = minimum(d₃,dims=1)[:] 48 | c₁ = cquantile(Chisq(2), .50) 49 | c₂ = cquantile(Chisq(2), .05) 50 | newl = findall(m₁ .> c₁) 51 | new₁ = findall(m₁[newl] .< c₂) 52 | ia = ia[setdiff(1:length(ia),co[newl[new₁]])] 53 | co = cc[co] 54 | 55 | if (length(newl) > 0 && length(cc) < a) || length(cc) == 0 56 | if length(cc) == 0 57 | newl = 1:k 58 | end 59 | ib = setdiff(1:a,ia) 60 | lst = lst[ib] 61 | @inbounds for j ∈ ib 62 | d₁[2j-1:2j,newl] = q[2j-1:2j,:]*f[2j-1:2j,newl] 63 | end 64 | if length(ib) > 0 65 | ind = [[2j-1,2j] for j ∈ ib] 66 | d₂ = d₁[vcat(ind...),newl] 67 | d₂ = d₂.^2 68 | d₃ = [d₂[i,j]+d₂[i+1,j] for i ∈ 1:2:2*length(ib),j ∈ 1:length(newl)] 69 | 70 | co₁ = argmin(d₃,dims=1) 71 | co₁ = co₁[:] 72 | co[newl] = lst[getindex.(co₁,1)] 73 | m₁ = minimum(d₃,dims=1)[:] 74 | new₁ = [] 75 | newl = newl[m₁ .> c₂] 76 | end 77 | end 78 | else 79 | newl = 1:k 80 | new₁ = [] 81 | end 82 | 83 | if length(new₁) > 0 84 | newl = newl[[i for i ∈ 1:length(newl) if i ∉ new₁]] 85 | end 86 | if length(newl) > 0 87 | co[newl] = n .+ [i for i ∈ 1:length(newl)] 88 | end 89 | return co 90 | end 91 | 92 | @everywhere include("markov_blanket.jl") 93 | 94 | @everywhere function myrange(q) 95 | idx = indexpids(q) 96 | if idx == 0 97 | return 1:0 98 | end 99 | nchunks = length(procs(q)) 100 | splits = [round(Int,s) for s ∈ range(0,stop=size(q,1)÷2,length=nchunks+1)] 101 | splits[idx]+1:splits[idx+1] 102 | end 103 | 104 | @everywhere function compute_cov!(Ω,lst,m₀,graph,q,jcb,qnoise,irange) 105 | @inbounds for i ∈ irange 106 | ind = markov_blanket(lst[i]+1,m₀.+1,graph) 107 | ind = ind[2:end] .- 1 108 | ind = [[2j+2,2j+3] for j ∈ ind ] 109 | ind = vcat(ind...) 110 | ind1 = findfirst(ind.==2*lst[i]+2) 111 | f = fxm(ind1,3+length(ind)) 112 | j = jcb[:,:,i] 113 | #drastic improvements with symmetric matrices specific solver. 114 | s = j*f / Symmetric(Ω[[1:3;ind],[1:3;ind]]) *f'*j' 115 | vals,vec = eigen(Symmetric(s+qnoise)) 116 | q[2i-1:2i,:] = diagm( 0 => sqrt.(1 ./ vals) )*vec' 117 | end 118 | end 119 | 120 | @everywhere function compute_cov_shared!(Ω,lst,m₀,graph,q,jcb,qnoise) 121 | compute_cov!(Ω,lst,m₀,graph,q,jcb,qnoise,myrange(q)) 122 | end 123 | 124 | @everywhere function fxm(p,n) 125 | sparse(1:5,[1,2,3,3+p,4+p],1,5,n) 126 | end 127 | 128 | function measure(θ) 129 | tmp = θ%2π 130 | tmp-min(tmp÷π,1)*2π 131 | end 132 | 133 | function intersect_ind(a,b,n) 134 | #assume a is sorted vectors of positive integers 135 | u = zeros(Int,2,n) 136 | v = zeros(Int,n) 137 | u[1,a] .= 1 138 | u[2,a] = 1:length(a) 139 | v[b] .= 1 140 | ps = u[1,:] .* v 141 | res = findall(ps .≠ 0) 142 | res, u[2,(ps .* u[2,:]) .> 0] 143 | end 144 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/data/aa3_dr.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch12_the_sparse_extended_information_filter/src/julia/data/aa3_dr.h5 -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/data/aa3_lsr2.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch12_the_sparse_extended_information_filter/src/julia/data/aa3_lsr2.h5 -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/data/z.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch12_the_sparse_extended_information_filter/src/julia/data/z.h5 -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/estimate.jl: -------------------------------------------------------------------------------- 1 | function estimate(m₀,μ,ξ,Ω) 2 | n = (length(μ)-3)÷2 3 | 4 | if n < 100 5 | μ .= Ω \ ξ 6 | else 7 | @inbounds for i ∈ m₀ 8 | ind = [2*i+2,2*i+3] 9 | μ[ind] = Ω[ind,ind] \ (ξ[ind]-Ω[ind,:]*μ+Ω[ind,ind]*μ[ind]) 10 | end 11 | μ[1:3] = Ω[1:3,1:3] \ (ξ[1:3]-Ω[1:3,:]*μ+Ω[1:3,1:3]*μ[1:3]) 12 | end 13 | end 14 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/markov_blanket.jl: -------------------------------------------------------------------------------- 1 | function markov_blanket(n,m₀,graph) 2 | sg = size(graph,1) 3 | mb1::Array{UInt16,1} = m₀ 4 | mb2::Array{UInt16,1} = findall(graph[n,:]) 5 | 6 | mb12 = intersect(mb1,mb2,sg) 7 | if length(mb12) == 0 8 | mb = bfs(n,graph) 9 | if length(mb) > 0 10 | mb2 = union(mb2,mb,sg) 11 | end 12 | end 13 | 14 | mb2 = [1;n;mb2] 15 | union(mb1,mb2,sg) 16 | end 17 | 18 | function Base.intersect(a::AbstractArray{UInt16,1},b::AbstractArray{UInt16,1},s::Int) 19 | x = zeros(Int,s) 20 | y = zeros(Int,s) 21 | x[a] .= 1 22 | y[b] .= 1 23 | findall((x .* y) .== 1) 24 | end 25 | 26 | function Base.union(a::AbstractArray{UInt16,1},b::AbstractArray{UInt16,1},s::Int) 27 | x = zeros(Int,s) 28 | y = zeros(Int,s) 29 | x[a] .= 1 30 | y[b] .= 1 31 | findall((x .+ y) .> 0) 32 | end 33 | 34 | function bfs(n,graph) 35 | sg = size(graph,1) 36 | d = -ones(Int,sg) 37 | d[1] = 0 38 | q = zeros(Int,sg) 39 | i = 1 40 | j = 2 41 | q[i] = 1 42 | found = false 43 | p = zeros(Int,0) 44 | while !found && i < j 45 | curr = q[i] 46 | i += 1 47 | next = findall(graph[n,:] .* (d.==-1)) 48 | s1 = length(next) 49 | q[j:j+s1-1] = next 50 | j += s1 51 | d[next] .= curr 52 | found=d[n]==curr 53 | end 54 | 55 | if found 56 | p = curr 57 | prev = d[curr] 58 | while prev ≠ 1 59 | p = [p prev] 60 | prev = d[prev] 61 | end 62 | end 63 | return p 64 | end 65 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/measurement.jl: -------------------------------------------------------------------------------- 1 | function measurement(z,c,μ,ξ,Ω,graph) 2 | n = size(graph,1)-1 3 | qnoise = diagm(0 => [5.0,0.02]) 4 | nnew = maximum(c)-n 5 | 6 | if nnew > 0 7 | osize = length(ξ) 8 | nsize = osize+2*nnew 9 | Ω₁ = zeros(nsize,nsize) 10 | Ω₁[1:osize,1:osize] = Ω 11 | Ω = Ω₁ 12 | append!(ξ,zeros(2*nnew)) 13 | append!(μ,inverse_measurement(μ[1:3],z[:,c .> n])[:]) 14 | padd = spzeros(Bool,nnew,n+1) 15 | padd1 = spzeros(Bool,n+nnew+1,nnew) 16 | graph = [[graph;padd] padd1] 17 | end 18 | 19 | jcb = jacobian_measurement(μ,c) 20 | ẑ = equation_measurement(μ,c) 21 | qnoiseinv = inv(qnoise) 22 | 23 | for (i,j) ∈ enumerate(c) 24 | h = jcb[:,:,i] 25 | dz = z[:,i]-ẑ[:,i] 26 | dz[2] = measure.(dz[2]) 27 | ind = [1:3;2j+2;2j+3] 28 | 29 | ξ[ind] .+= h'*qnoiseinv*(dz+h*μ[ind]) 30 | Ω[ind,ind] .+= h'*qnoiseinv*h 31 | end 32 | 33 | c = unique(c) 34 | rows = ones(length(c)) 35 | cols = c .+ 1 36 | sg = size(graph,1) 37 | adjacency = sparse(rows,cols,true,sg,sg) 38 | graph .|= adjacency 39 | graph .|= adjacency' 40 | 41 | return Ω,graph 42 | 43 | end 44 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/motion.jl: -------------------------------------------------------------------------------- 1 | function motion(v,α,dt,m₀,μ,ξ,Ω,graph) 2 | 3 | n = length(ξ) 4 | rnoise = diagm(0 => [8e-4,8e-4,1e-6]) 5 | jcb = jacobian_motion(μ[3],v,α,dt) 6 | 7 | ψ = inv(jcb)-I 8 | ind = [[2*i+2;2*i+3] for i ∈ sort(m₀)] 9 | ind = vcat(ind...) 10 | ind = [1:3;ind] 11 | Ω₁ = Ω[ind,1:3] 12 | ψ₁ = Ω₁*ψ 13 | 14 | λ = zeros(n,n) 15 | λ[ind,1:3] = ψ₁ 16 | λ[1:3,ind] .+= ψ₁' 17 | λ[1:3,1:3] .+= ψ'*Ω[1:3,1:3]*ψ 18 | Φ = Ω + λ 19 | 20 | Φₓ = Φ[1:3,ind] 21 | κ = zeros(n,n) 22 | κ[ind,ind] = Φₓ'*inv(inv(rnoise)+Φ[1:3,1:3])*Φₓ 23 | Ω .+= λ - κ 24 | 25 | δ = equation_motion(μ[3],v,α,dt) 26 | ξ[ind] .+= Ω[ind,1:3]*δ + (λ-κ)[ind,:]*μ 27 | μ[1:3] += δ 28 | 29 | if length(m₀)>0 30 | edges = [[i+1 j+1] for i ∈ m₀,j ∈ m₀ if i ≠ j] 31 | edges = vcat(edges...) 32 | if size(edges,1)>0 33 | sg = size(graph,1) 34 | adjacency = sparse(edges[:,1],edges[:,2],true,sg,sg) 35 | graph .|= adjacency 36 | end 37 | end 38 | end 39 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/seif.jl: -------------------------------------------------------------------------------- 1 | using LinearAlgebra 2 | using LinearModel 3 | using SparseArrays, SharedArrays 4 | using HDF5 5 | using Profile 6 | using Distributed 7 | using Distributions 8 | using Plots 9 | using Printf 10 | 11 | macro print_array(q) 12 | return :(Base.print_array(stdout,round.( $(esc(q)) ,sigdigits=4));println()) 13 | end 14 | 15 | include("seif_fct.jl") 16 | 17 | function seif() 18 | filename1 = "aa3_lsr2.h5" 19 | filename2 = "aa3_dr.h5" 20 | filename3 = "z.h5" 21 | include_dir = "./data/" 22 | 23 | dt = 25e-3 24 | graph = spzeros(Bool,1,1) 25 | qnoise = diagm(0 => [5, 0.02]) 26 | 27 | speed, steering, timeit = h5open(include_dir * filename2, "r") do file 28 | (read(file, "/speed"), read(file, "/steering"), read(file, "/time")) 29 | end 30 | 31 | timelsr = h5open(include_dir * filename1, "r") do file 32 | read(file, "/timeLsr") 33 | end 34 | llsr = size(timelsr,1) 35 | 36 | z = h5open(include_dir * filename3, "r") do file 37 | read(file, "/zz") 38 | end 39 | 40 | μ = zeros(3) 41 | ξ = zeros(3) 42 | Ω = 10e4*diagm(0 => [1,1,1]) 43 | m₀ = zeros(Int,0) 44 | n = 20 45 | 46 | stindex = 1 47 | j = findfirst(timelsr .>= timeit[stindex])[1] 48 | 49 | gr(reuse=true) 50 | plt = plot([0;.1],[0;.2];axis=false, 51 | leg=false, 52 | aspect_ratio=1, 53 | grid=false) 54 | xlims!((-200,200)) 55 | ylims!((-50,300)) 56 | 57 | poses = zeros(3,5000) 58 | #ξ_hist = zeros(3,500) 59 | t₁ = time() 60 | 61 | for i ∈ stindex:size(timeit,1) 62 | t₂ = time() 63 | 64 | if i > stindex && i%5000==0 65 | plot!(poses[1,:],poses[2,:],line = (:black, 1, 0.2)) 66 | scatter!(μ[4:2:end],μ[5:2:end], 67 | markersize = 1 ) 68 | Plots.gui() 69 | h5open(include_dir * "iter_$i.h5", "w") do file 70 | write(file, "/poses", poses) 71 | write(file, "/μ", μ) 72 | write(file, "/ξ", ξ) 73 | write(file, "/Ω", Ω) 74 | write(file, "/m₀", m₀) 75 | #write(file, "/ξ_hist", ξ_hist) 76 | end 77 | poses = zeros(3,5000) 78 | #ξ_hist = zeros(3,500) 79 | end 80 | 81 | motion(speed[i],steering[i],dt,m₀,μ,ξ,Ω,graph) 82 | 83 | estimate(m₀,μ,ξ,Ω) 84 | 85 | m₃ = zeros(Int,0) 86 | while j ≤ llsr && timelsr[j] < timeit[i]+dt*1000 87 | z₁ = z[2*j-1:2*j,z[2*j,:] .≠ 0] 88 | if !isempty(z₁) 89 | c = correspondence(z₁,m₀,μ,Ω,graph) 90 | Ω,graph = measurement(z₁,c,μ,ξ,Ω,graph) 91 | m₃ = sort(union(m₃,c)) 92 | end 93 | j += 1 94 | end 95 | if !isempty(m₃) 96 | m₄ = sort(setdiff(m₀,m₃)) 97 | q = [m₄;unique(m₃)] 98 | tmp = q[max(1,length(q)-n+1):end] 99 | m₁ = setdiff(m₀,tmp) 100 | m₁ = union(m₁,setdiff(m₃,tmp)) 101 | m₀ = tmp 102 | if !isempty(m₁) 103 | sparsification(m₀,m₁,μ,ξ,Ω,graph) 104 | end 105 | end 106 | 107 | t₃ = time() 108 | println("\33[2J") 109 | @printf "iter: %d\n" i 110 | @printf "iter time: %.5f\n" t₃-t₂ 111 | @printf "avg: %.5f\n" ((t₃-t₁)/(i-stindex+1)) 112 | 113 | poses[:,1+i%5000] = μ[1:3] 114 | end 115 | end #function seif() 116 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/seif_fct.jl: -------------------------------------------------------------------------------- 1 | include("motion.jl") 2 | include("estimate.jl") 3 | include("correspondence.jl") 4 | include("measurement.jl") 5 | include("sparsification.jl") 6 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/seif_launch.jl: -------------------------------------------------------------------------------- 1 | using Distributed 2 | const N_CORES = 7 3 | rmprocs(workers()) 4 | addprocs(N_CORES) 5 | @everywhere push!(LOAD_PATH,".") 6 | @everywhere using SparseArrays, LinearAlgebra, SharedArrays 7 | include("seif.jl") 8 | 9 | seif() 10 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/julia/sparsification.jl: -------------------------------------------------------------------------------- 1 | function sparsification(m₀,m₁,μ,ξ,Ω,graph) 2 | 3 | sg = size(graph,1) 4 | rows = ones(length(m₁)) 5 | cols = m₁ .+ 1 6 | adjacency = sparse(rows,cols,true,sg,sg) 7 | graph .⊻= adjacency .| adjacency' 8 | 9 | edges = [[i+1 j+1] for i ∈ union(m₀,m₁),j ∈ m₁ if i ≠ j] 10 | edges = vcat(edges...) 11 | adjacency = sparse(edges[:,1],edges[:,2],true,sg,sg) 12 | graph .|= adjacency .| adjacency' 13 | 14 | ind₀ = [[2i+2,2i+3] for i ∈ sort(m₀)] 15 | ind₀ = vcat(ind₀...) 16 | ind₁ = [[2i+2,2i+3] for i ∈ sort(m₁)] 17 | ind₁ = vcat(ind₁...) 18 | 19 | Ω₂ = Ω[[1:3;ind₀;ind₁],ind₁] 20 | l₁ = Ω₂ / Symmetric(Ω[ind₁,ind₁]) * Ω₂' 21 | 22 | Ω₃ = Ω[[1:3;ind₀;ind₁],[1:3;ind₁]] 23 | l₂ = Ω₃ / Symmetric(Ω[[1:3;ind₁],[1:3;ind₁]]) * Ω₃' 24 | 25 | Ω₄ = Ω[:,1:3] 26 | l₃ = Ω₄ / Symmetric(Ω[1:3,1:3]) * Ω₄' 27 | 28 | Ω₁ = copy(Ω) 29 | Ω[[1:3;ind₀;ind₁],[1:3;ind₀;ind₁]] .-= l₁ 30 | Ω[[1:3;ind₀;ind₁],[1:3;ind₀;ind₁]] .+= l₂ 31 | Ω .-= l₃ 32 | 33 | ξ .+= (Ω-Ω₁)*μ 34 | end 35 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/SEIF.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %Sparse extended information filter 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | % ============================================================= 7 | function SEIF 8 | 9 | fileName1 = 'aa3_lsr2.mat' ; 10 | fileName2 = 'aa3_dr.mat' ; 11 | 12 | 13 | %file1 = 'landmarks_20000.mat' ; 14 | %file2 = 'poses_20000.mat' ; 15 | %file2b = 'poses_20000.mat' ; 16 | %file3 = 'm0_20000.mat' ; 17 | %file4 = 'xi_20000.mat' ; 18 | %file5 = 'omega_20000.mat' ; 19 | %file6 = 'G_20000.mat' ; 20 | 21 | 22 | dt = 25e-3; 23 | Mask13 = uint16(2^13 -1) ; 24 | 25 | global G; 26 | G=sparse(1,1); 27 | 28 | global Q; 29 | Q= diag([5 0.02]); 30 | load(fileName1) ; 31 | L = size(LASER);L=L(1) ; 32 | timeLsr = double(TLsr) ; clear TLsr; j=1; 33 | 34 | load(fileName2) ; 35 | 36 | global m xi O 37 | m=zeros(3,1); 38 | xi=zeros(3,1); 39 | O=10e4*eye(3); 40 | m0 = zeros(1,0); 41 | 42 | N = 20;%max active landmarks 43 | 44 | % figure(1) ;clf ; 45 | % zoom on ; 46 | %hold on; 47 | %hhh2=plot(G) ; 48 | %axis([-50,50,0,75]); 49 | %hold off ; 50 | 51 | % figure(1) ;clf ; 52 | % zoom on ; 53 | % hhh =plot(0,0,'.','erasemode','xor') ; %laser 54 | % hold on; 55 | % hhh2=plot(0,0,'+','erasemode','xor','MarkerSize',2) ; % landmarks centers 56 | % hhh3=plot(0,0,'o','erasemode','xor','MarkerSize',2) ; % approx. landm. circles 57 | % axis([-200,200,-50,350]); 58 | % hold off ; 59 | 60 | % G=importdata(file6); 61 | % O=importdata(file5); 62 | % xi=importdata(file4); 63 | % m=importdata(file1); 64 | % m0=importdata(file3); 65 | % poses=importdata(file2); 66 | % poses1=importdata(file2b); 67 | % set(hhh2,'XData',[poses(1,:) poses1(1,:)],'YData',[poses(2,:) poses1(2,:)]) ; 68 | % clearvars poses poses1; 69 | 70 | poses=zeros(3,5000); 71 | pause(2); 72 | 73 | tstart1=tic; 74 | stindex=20000; 75 | 76 | for i=stindex:size(time); 77 | 78 | j=find(timeLsr>=time(i),1); 79 | tstart2=tic; 80 | 81 | if i>stindex && mod(i,5000)==0 82 | save(['poses_' num2str(i) '.mat'],'poses'); 83 | clearvars poses; 84 | poses=zeros(3,5000); 85 | 86 | save(['landmarks_' num2str(i) '.mat'],'m'); 87 | save(['xi_' num2str(i) '.mat'],'xi'); 88 | save(['omega_' num2str(i) '.mat'],'O'); 89 | save(['m0_' num2str(i) '.mat'],'m0'); 90 | save(['G_' num2str(i) '.mat'],'G'); 91 | end 92 | 93 | SEIF_motion(speed(i),steering(i),dt,m0); 94 | 95 | SEIF_estimate(m0); 96 | 97 | while j <= L && timeLsr(j) < time(i)+dt*1000 98 | m3 = zeros(1,0);c=zeros(1,0); 99 | raw = double( bitand( Mask13,LASER(j,:)) ) ; 100 | raw = raw/100; 101 | z = detectTreesI16(raw); 102 | if ~isempty(z) 103 | z = z(1:2,:); 104 | c = SEIF_correspondence(z,m0); 105 | SEIF_measurement(z,c); 106 | end 107 | j=j+1; 108 | m3 = union(m3,c); 109 | 110 | if ~isempty(m3) 111 | n=(size(xi,1)-3)/2; 112 | 113 | m0a = setdiff(m0,m3); 114 | q = [m0a unique(m3)]; 115 | 116 | tmp=q(1,max(1,size(q,2)-N+1):end); 117 | 118 | m1 = setdiff(m0,tmp); 119 | m1 = union(m1,setdiff(m3,tmp)); 120 | m0=tmp; 121 | m2 = setdiff(setdiff(1:n,m1),m0); 122 | if ~isempty(m1) 123 | SEIF_sparsification(m0,m1,m2); 124 | end 125 | end 126 | end 127 | 128 | titer1=toc(tstart1);titer2=toc(tstart2);clc; 129 | fprintf('iter: %d\n', i); 130 | fprintf('iter time: %.5f\n',titer2 ); 131 | fprintf('avg: %.5f\n', titer1/(i-stindex+1)); 132 | poses(:,1+mod(i,5000))=m(1:3); 133 | 134 | % if mod(i,200)==0 135 | % set(hhh3,'XData', m(2:2:end)','YData',m(3:2:end)') ; 136 | % end 137 | % title( num2str(i) ); 138 | % set(hhh2,'XData',[get(hhh2,'XData') m(1)],'YData',[get(hhh2,'YData') m(2)]) ; 139 | % drawnow ; 140 | 141 | end 142 | end 143 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/SEIF_correspondence.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | % SEIF correspondence determination 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %z: N x 2 measurements 7 | 8 | %c: indexes of landmarks corresponding to observations 9 | 10 | % ============================================================= 11 | 12 | function [ c ] = SEIF_correspondence( z,m0) 13 | 14 | 15 | global G;global Q; 16 | 17 | global m xi O 18 | k = size(z,2); 19 | if k==0 20 | c=zeros(1,0); 21 | return; 22 | end 23 | c= zeros(1,k); 24 | n = (size(xi,1)-3)/2; 25 | 26 | if n > 0 27 | 28 | %rough landmarks selection 29 | t = m(4:end)';t1=t(1:2:end);t2=t(2:2:end); 30 | lst = find(abs(t1-m(1))<=75 ... 31 | & abs(t2-m(2))<=75 ... 32 | & (t1-m(1))*cos(m(3))+(t2-m(2))*sin(m(3))>=-5); 33 | a=size(lst,2); 34 | A = zeros(2*a,2); 35 | 36 | J = jacobian_measurement(m,lst); 37 | zhat = reshape(equation_measurement(m,lst),2,1,a); 38 | 39 | for ii=1:a 40 | 41 | ind = markov_blanket(lst(ii),m0); 42 | 43 | ind=ind(2:end); 44 | c1 = 2 + 2*ind;c2=c1+1;ind=reshape([c1;c2],[1,2*size(ind,2)]); 45 | 46 | ind1 = find(ind==2+2*lst(ii)); 47 | 48 | F=Fxm(ind1,3+size(ind,2)); 49 | S = J(:,:,ii)*F/O([1:3 ind],[1:3 ind]); 50 | 51 | [U,M1]=eig(S*F'*J(:,:,ii)'+Q); 52 | U = diag(sqrt(1./diag(M1)))*U'; 53 | A(2*ii-1:2*ii,:) = U; 54 | 55 | clear F; 56 | 57 | end 58 | 59 | F = reshape(permute(bsxfun(@minus,zhat,z),[1,3,2]),2*a,k); 60 | F(2:2:end,:)=measure(F(2:2:end,:)); 61 | %first pass over active landmarks 62 | N = zeros(2*a,k); 63 | [C,ia,~] = intersect(lst,m0,'stable'); 64 | 65 | for j=1:size(C,2) 66 | jj=ia(j); 67 | N(2*jj-1:2*jj,:)= A(2*jj-1:2*jj,:)*F(2*jj-1:2*jj,:); 68 | end 69 | N1 = N(reshape([2*ia'-1;2*ia'],1,[]),:); 70 | N1 = N1.^2;M2=N1(2:2:end,:);N1 = N1(1:2:end,:)+M2; 71 | [m1, c] = min(N1,[],1);%search min along first dim, even if row vector. 72 | 73 | new = find(m1>chi2inv(0.5,2)); 74 | new1 = find(m1(new)chi2inv(0.95,2))); 93 | new1=zeros(1,0); 94 | end 95 | end 96 | else 97 | new = 1:k; 98 | new1=zeros(1,0); 99 | end 100 | if ~isempty(new1) 101 | new = new(setdiff(1:size(new,2),new1)); 102 | end 103 | 104 | if ~isempty(new) 105 | c(1,new) = n + (1:(size(new,2))); 106 | end 107 | 108 | end 109 | 110 | function alpha = measure(theta) 111 | tmp =mod(theta,2*pi); 112 | %matlab mod may return 2pi due to precision limitation 113 | alpha = tmp-min(floor(tmp/pi),1)*2*pi; 114 | end 115 | 116 | function [ F ] =Fxm(p,n) 117 | 118 | if p>0 119 | F=sparse(1:5, [1 2 3 3+p 4+p],ones(1,5),5,n); 120 | else if n==0 121 | F=sparse(1:3,1:3,ones(1,3),3,n); 122 | end 123 | end 124 | end 125 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/SEIF_estimate.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %SEIF update state mean 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | 7 | %m0: index active landmarks 8 | 9 | % ============================================================= 10 | function [ ] = SEIF_estimate(m0) 11 | 12 | global xi O m 13 | n=(size(xi,1)-3)/2; 14 | 15 | if n < 100 16 | m=O\xi; 17 | else 18 | 19 | for i=1:size(m0,2) 20 | m(2*m0(i)+2:2*m0(i)+3) = (O(2*m0(i)+2:2*m0(i)+3,2*m0(i)+2:2*m0(i)+3))... 21 | \(xi(2*m0(i)+2:2*m0(i)+3)... 22 | -O(2*m0(i)+2:2*m0(i)+3,:)*m... 23 | +O(2*m0(i)+2:2*m0(i)+3,2*m0(i)+2:2*m0(i)+3)*m(2*m0(i)+2:2*m0(i)+3)); 24 | end 25 | m(1:3) = (O(1:3,1:3))\(xi(1:3)-O(1:3,:)*m+O(1:3,1:3)*m(1:3)); 26 | 27 | end 28 | end 29 | 30 | function [ F ] =Fm(p,n) 31 | if p>0 32 | F=sparse(1:2, [2*p+2 2*p+3],ones(1,2),2,n); 33 | else if p==0 34 | F=sparse(1:3, 1:3,ones(1,3),3,n); 35 | end 36 | end 37 | end -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/SEIF_measurement.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | % SEIF measurement update 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %xi: information vector 7 | %O: information matrix 8 | %m: current mean estimate 9 | %z: N x 2 measurements 10 | %c: correspondence N x 1 11 | 12 | % ============================================================= 13 | function [] = SEIF_measurement( z, c) 14 | global G;global Q; 15 | global m xi O 16 | 17 | n = (size(xi,1)-3)/2; 18 | new = max(c)-n; 19 | 20 | if new > 0 21 | O1 = zeros(size(xi,1)+2*new,size(xi,1)+2*new); 22 | O1(1:size(xi,1),1:size(xi,1))= O; 23 | xi1 = zeros(size(xi,1)+2*new,1); 24 | xi1(1:size(xi,1),1) = xi; 25 | m1 = zeros(size(xi,1)+2*new,1); 26 | m1(1:size(xi,1),1) = m; 27 | m1(size(xi,1)+1:end,1) = reshape(inverse_measurement(m(1:3),z(:,c>n)),[2*new,1]); 28 | s=size(G,1); 29 | G(s+new,s+new)=0; 30 | else 31 | O1 = O; 32 | xi1 = xi; 33 | m1=m; 34 | end 35 | 36 | J = jacobian_measurement(m1,c); 37 | zhat = equation_measurement(m1,c); 38 | 39 | for ii=1:size(c,2) 40 | 41 | jj = c(ii); 42 | H = J(:,:,ii); 43 | dz = z(:,ii)-zhat(:,ii); 44 | dz(2) = measure(dz(2)); 45 | xi1([1:3 2*jj+2 2*jj+3],1) = xi1([1:3 2*jj+2 2*jj+3],1) + H'*inv(Q)*(dz+H*m1([1:3 2*jj+2 2*jj+3],1)); 46 | O1([1:3 2*jj+2 2*jj+3],[1:3 2*jj+2 2*jj+3]) = O1([1:3 2*jj+2 2*jj+3],[1:3 2*jj+2 2*jj+3])+ H'*inv(Q)*H; 47 | %change inv Q \ 48 | 49 | end 50 | m=m1; 51 | xi=xi1; 52 | %O=sparse(O1); 53 | O=O1; 54 | %unneeded in theory 55 | c = unique(c); 56 | G=G | sparse(ones(1,size(c,2)), c+1,ones(1,size(c,2)),(size(xi,1)-3)/2+1,(size(xi,1)-3)/2+1); 57 | 58 | 59 | end 60 | 61 | function alpha = measure(theta) 62 | tmp =mod(theta,2*pi); 63 | %there is a bug in matlab mod 64 | alpha = tmp-min(floor(tmp/pi),1)*2*pi; 65 | end -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/SEIF_motion.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | % SEIF motion update 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %xi: initial information vector 7 | %O: initial information matrix 8 | %m: initial mean 9 | %v: speed 1 x 3 10 | %a: steering 1 11 | %t: time step 12 | %m0: active landmarks 13 | 14 | % ============================================================= 15 | function [ ] = SEIF_motion(v, a, t,m0) 16 | 17 | global G; 18 | 19 | global m xi O 20 | 21 | n = size(xi,1); 22 | 23 | R= diag([8e-4 8e-4 1e-6]); 24 | J = jacobian_motion(m(3,1), v, a, t); 25 | 26 | psi = (inv(J)-eye(3)); 27 | 28 | OX=sparse(O(:, 1:3)); 29 | psi1 = OX*psi; 30 | 31 | lambda=sparse(n,n); 32 | lambda(:, 1:3) = psi1;lambda(1:3,:) = lambda(1:3,:) + psi1'; 33 | lambda(1:3, 1:3)= lambda(1:3, 1:3) + psi'*O(1:3,1:3)*psi; 34 | 35 | phi = O + lambda; 36 | phiX=sparse(phi(:,1:3)); 37 | k = phiX*inv(inv(R)+ phi(1:3,1:3))*phiX'; 38 | O = phi-k; 39 | 40 | dx = equation_motion(m(3,1), v, a, t); 41 | OX=sparse(O(:, 1:3)); 42 | xi = xi + lambda*m-k*m+OX*dx; 43 | m = m + [dx;zeros(n-3,1)]; 44 | 45 | clear OX 46 | 47 | if ~isempty(m0) 48 | [s,t]=meshgrid(m0); 49 | s=s(:)'+1;t=t(:)'+1; 50 | edges=unique(sort([s;t],1)','rows')'; 51 | edges=edges(:,edges(1,:)-edges(2,:)~=0); 52 | if ~isempty(edges) 53 | G=G | sparse(edges(1,:),edges(2,:) ,size(edges,2),size(G,1),size(G,1)); 54 | end 55 | end 56 | 57 | end -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/SEIF_sparsification.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | % SEIF sparsification 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %xi: information vector 7 | %O: information matrix 8 | %m0: index active landmarks 9 | %m1: index landmarks to be deactivated 10 | %m2: index passive landmarks 11 | 12 | % ============================================================= 13 | function [ ] = SEIF_sparsification( m0,m1,m2) 14 | 15 | global m xi O 16 | 17 | global G; 18 | sg=size(G,1); 19 | mask=ones(sg,sg)-sparse(ones(1,size(m1,2)),m1+1,ones(1,size(m1,2)),sg,sg); 20 | G= G & mask; 21 | [s,t]=meshgrid(union(m1,m0),m1); 22 | s=s(:)'+1;t=t(:)'+1; 23 | edges=unique(sort([s;t],1)','rows')'; 24 | edges=edges(:,edges(1,:)-edges(2,:)~=0); 25 | G=G | sparse(edges(1,:),edges(2,:) ,size(edges,2),sg,sg); 26 | 27 | s0=size(m0,2);s1=size(m1,2);s2=size(m2,2);s=size(m,1); 28 | m0=sort(m0,2);m1=sort(m1,2);m2=sort(m2,2); 29 | m0 = 2 + 2*m0; 30 | m0 = reshape([m0;m0+1],1,[]); 31 | m1 = 2 + 2*m1; 32 | m1 = reshape([m1;m1+1],1,[]); 33 | s0=size(m0,2);s1=size(m1,2);s2=size(m2,2);s=size(m,1); 34 | 35 | O2 = O([1:3 m0 m1],m1); 36 | L1 = O2/O(m1,m1)*O2'; 37 | O3=O([1:3 m0 m1],[1:3 m1]); 38 | L2 = O3/O([1:3 m1],[1:3 m1])*O3'; 39 | O4=O(:,1:3); 40 | L3 = O4/O(1:3,1:3)*O4'; 41 | 42 | O1 = O; 43 | O1([1:3 m0 m1],[1:3 m0 m1]) = O1([1:3 m0 m1],[1:3 m0 m1])-L1+L2; 44 | %O1([1:3 m0 m1],[1:3 m0 m1]) = O1([1:3 m0 m1],[1:3 m0 m1])+L2; 45 | O1 = O1 - L3; 46 | 47 | %reset to 0 for sparse conversion 48 | O1(1:3,m1)=zeros(3,size(m1,2)); 49 | O1(m1 ,1:3)=zeros(size(m1,2),3); 50 | 51 | 52 | xi = xi + (O1-O)*m; 53 | O=O1; 54 | 55 | end -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/equation_measurement.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %equations of measurement model 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %m:current mean estimate 7 | %c: correspondance vector 8 | 9 | %F: measurement model 2 x n 10 | 11 | % ============================================================= 12 | function [ F ] = equation_measurement(m,c) 13 | 14 | n=size(c,2); 15 | c1 = 2 + 2*c'; 16 | x = m(c1);y = m(c1+1); 17 | 18 | delta = bsxfun(@minus ,[x';y'], m(1:2)); 19 | F = [sqrt(sum(delta.^2));zeros(1,n)]; 20 | F(2,:) = atan2(delta(2,:),delta(1,:))-m(3)+pi/2; 21 | 22 | end -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/equation_motion.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | % equations of motion model 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %theta: robot orientation 7 | %v: speed 1 8 | %a: steering 1 9 | %t: time step 10 | 11 | %F: discrete motion model advance 12 | 13 | % ============================================================= 14 | function [ J ] = equation_motion(theta, v, a, t) 15 | 16 | %car parameters 17 | H=0.74; 18 | L=2.83; 19 | a1= 0.95+L; b1=0.5; 20 | 21 | v=v/(1-tan(a)*H/L); 22 | 23 | J = [ t*(v*cos(theta) - v/L*tan(a)*(a1*sin(theta)+b1*cos(theta))); 24 | t*(v*sin(theta)+v/L*tan(a)*(a1*cos(theta)-b1*sin(theta))); 25 | t*v/L*tan(a)]; 26 | 27 | end -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/inverse_measurement.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %inverse of measurement function 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %m:current robot position estimate 7 | %c: vector of sensings new landmarks 8 | 9 | %X: initialization of new landmarks positions 10 | 11 | % ============================================================= 12 | function [ X ] = inverse_measurement(m,z) 13 | 14 | C = cos(m(3,1)+z(2,:)); 15 | S = sin(m(3,1)+z(2,:)); 16 | 17 | X = bsxfun(@plus,m(1:2,1), [z(1,:).*S;-z(1,:).*C]); 18 | 19 | end -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/jacobian_measurement.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | % jacobian measurement model 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %m: current mean estimate 7 | %c: correspondence N x 1 8 | 9 | %J: jacobian of measurement model 2 x 5 x N 10 | 11 | % ============================================================= 12 | function [ J ] = jacobian_measurement(m,c) 13 | 14 | N = size(c,2); 15 | c1 = 2 + 2*c'; 16 | x = m(c1);y = m(c1+1); 17 | 18 | delta = bsxfun(@minus ,[x';y'], m(1:2)); 19 | q = sum(delta.^2); 20 | 21 | J = [1 0;0 -1;0 1;+1 0]*delta; 22 | J = [-J;zeros(1,N);-ones(1,N);J]; 23 | J(1:2:end,:,:) = bsxfun(@times, J(1:2:end,:,:), 1./sqrt(q)); 24 | J([2 4 8 10],:,:) = bsxfun(@times, J([2 4 8 10],:,:), 1./(q)); 25 | 26 | J = reshape(J,2,5,N); 27 | end -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/jacobian_motion.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | % jacobian motion model 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %theta: robot orientation 7 | %v: speed 1 x 3 8 | %a: steering 1 9 | %t: time step 10 | 11 | %J: jacobian of motion model 12 | 13 | % ============================================================= 14 | function [ J ] = jacobian_motion(theta, v, a, t) 15 | 16 | %car parameters 17 | H=0.74; 18 | L=2.83; 19 | a1= 0.95+L; b1=0.5; 20 | 21 | J = [1 0 -t*(v*sin(theta) + v/L*tan(a)*(a1*cos(theta)-b1*sin(theta))); 22 | 0 1 t*(v*cos(theta)-v/L*tan(a)*(a1*sin(theta)+b1*cos(theta))); 23 | 0 0 1]; 24 | 25 | end -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/matlab/markov_blanket.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %compute combined markov blanket of robot position and landmark 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %n: index of landmark (starts from 1) 7 | 8 | %mb: list of landmarks in combined markov blanket 9 | 10 | % ============================================================= 11 | function [ mb ] = markov_blanket(n,m0) 12 | 13 | global G; 14 | 15 | mb1 = m0+1; 16 | mb2 = find(G(n+1,:) | G(:,n+1)'); 17 | 18 | if isempty(intersect_int(mb1,mb2)) 19 | mb = bfs(n+1);%%%%%%%%%%%%%%%this shoul not be empty 20 | if ~isempty(mb) 21 | mb2 = union_int( mb2,mb); 22 | end 23 | end 24 | 25 | mb2 = [1 n+1 mb2]; 26 | mb = union_int(mb1,mb2); 27 | mb=mb-1; 28 | 29 | end 30 | 31 | 32 | function [ z ] = union_int(m1,m2) 33 | global xi; 34 | 35 | s=(size(xi,1)-3)/2+1; 36 | x=zeros(1,s); 37 | y=zeros(1,s); 38 | x(m1) = 1; 39 | y(m2) = 1; 40 | z = find(x | y); 41 | 42 | 43 | end 44 | 45 | function [ z ] = intersect_int(m1,m2) 46 | global xi; 47 | s=(size(xi,1)-3)/2+1; 48 | x=zeros(1,s); 49 | y=zeros(1,s); 50 | x(m1) = 1; 51 | y(m2) = 1; 52 | z = find(x & y); 53 | 54 | end 55 | 56 | function [ p ] = bfs(n) 57 | 58 | global G; 59 | sg=size(G,1); 60 | d=-ones(1,sg); 61 | q = zeros(1,sg);i=1;j=2;q(1)=1; 62 | found=false; 63 | p=zeros(1,0); 64 | curr=1; 65 | while ~found && i0: 24 | 25 | t = m[3:] 26 | t1 = t[0::2] 27 | t2 = t[1::2] 28 | lst = nonzero(logical_and(logical_and(abs(t1-m[0])<= 75\ 29 | , abs(t2-m[1])<= 75)\ 30 | , (t1-m[0])*cos(m[2])+(t2-m[1])*sin(m[2]) >= -5 ))[0] 31 | a = lst.size 32 | aa = zeros([2*a,2]) 33 | 34 | J = jacobian_measurement(m,lst+1) 35 | zhat = equation_measurement(m,lst+1).reshape((2,1,a)) 36 | 37 | for i in range(0,a): 38 | ind = markov_blanket(lst[i]+1,m0,G) 39 | ind = ind[1:]-1 40 | c1 = 3+2*ind 41 | ind = vstack((c1,c1+1)).flatten(order='F') 42 | ind1 = nonzero(ind==3+2*lst[i])[0][0] 43 | f = fxmd(ind1,3+ind.size) 44 | ind2 = np.hstack((r_[0:3],ind)) 45 | ind3 = np.hstack((r_[0:3],array([3+ind1,4+ind1]))) 46 | 47 | s = la.solve(omega[ix_(ind2,ind2)],f.T@J[:,:,i].T) 48 | d,u = la.eigh(J[:,:,i]@s[ind3,:]+Q) 49 | u = diag(sqrt(1/d)).dot(u.T) 50 | aa[2*i:2*i+2,:] = u 51 | 52 | f = np.transpose(zhat-z.reshape((2,k,1)),(2,0,1)).reshape((2*a,k)) 53 | f[1::2,:] = measure(f[1::2,:]) 54 | 55 | d1 = zeros([2*a,k]); 56 | ia = arange(a)[in1d(lst, m0-1)] 57 | cc = lst[ia] 58 | 59 | for j in range(0,cc.size): 60 | ind=ia[j] 61 | d1[2*ind:2*ind+2,:] = aa[2*ind:2*ind+2,:].dot(f[2*ind:2*ind+2,:]) 62 | 63 | d2 = d1[vstack((2*ia,2*ia+1)).flatten(order='F'),:] 64 | d2 = d2**2 65 | m2 = d2[1::2,:] 66 | d3 = d2[0::2,:] + m2 67 | 68 | m1 = amin(d3,axis=0) 69 | co = argmin(d3,axis=0) 70 | new = nonzero(m1>chi2.ppf(0.50,2))[0] 71 | new1 = nonzero(m1[new]0 and cc.size0: 87 | d2 = d1[ix_(vstack((2*ib,2*ib+1)).flatten(order='F'),new)] 88 | d2 = d2**2 89 | m2 = d2[1::2,:] 90 | d3 = d2[0::2,:] + m2 91 | m1 = amin(d3,axis=0) 92 | co[new] = argmin(d3,axis=0) 93 | 94 | co[new] = lst[co[new]]+1 95 | new = new[m1>chi2.ppf(0.95,2)] 96 | new1 = zeros(0) 97 | else: 98 | new = r_[0:k] 99 | new1 = zeros(0) 100 | 101 | if new1.size>0: 102 | new = new[setdiff1d(r_[0:new.size],new1)] 103 | if new.size>0: 104 | co[new] = n+ r_[1:new.size+1] 105 | 106 | return co 107 | 108 | def fxmd(p,n): 109 | if p>=0: 110 | res=zeros([5,n]) 111 | res[0:3,0:3] = eye(3) 112 | res[3,3+p] = 1 113 | res[4,4+p] = 1 114 | return res 115 | 116 | def measure(theta): 117 | tmp = theta%(2*pi) 118 | return tmp-minimum(floor(tmp/pi),1)*2*pi 119 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/equation_measurement.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | 3 | def equation_measurement(m,ind): 4 | N = ind.size 5 | ind1 = 1+2*ind 6 | x = m[ind1] 7 | y = m[ind1+1] 8 | 9 | delta = vstack((x,y))-m[0:2].reshape((2,1)) 10 | q = sqrt(sum(delta**2,axis=0)) 11 | return vstack((q,arctan2(delta[1,:],delta[0,:])-m[2]+pi/2)) 12 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/equation_motion.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | 3 | def equation_motion(theta,v,a,dt): 4 | H = 0.74 5 | L = 2.83 6 | a1 = 0.95 + L 7 | b1 = 0.5 8 | 9 | v = v/(1-tan(a)*H/L) 10 | 11 | return array([dt*(v*cos(theta)-v/L*tan(a)*(a1*sin(theta)+b1*cos(theta))),\ 12 | dt*(v*sin(theta)+v/L*tan(a)*(a1*cos(theta)-b1*sin(theta))),\ 13 | dt*v/L*tan(a)]) 14 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/estimate.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | from scipy.linalg import solve 3 | 4 | from pdb import set_trace as bp 5 | 6 | def estimate(m0,xi,omega,m): 7 | n = (xi.size-3)//2 8 | 9 | if n < 100: 10 | 11 | m1 = solve(omega,xi) 12 | 13 | else: 14 | m1=m 15 | for i in range(0,m0.size): 16 | ind = 2*m0[i]+1 17 | 18 | m1[ind:ind+2] = solve(omega[ind:ind+2,ind:ind+2],xi[ind:ind+2]-omega[ind:ind+2,:].dot(m1)+omega[ind:ind+2,ind:ind+2].dot(m1[ind:ind+2])) 19 | m1[0:3] = solve(omega[0:3,0:3],xi[0:3]-omega[0:3,:].dot(m1)+omega[0:3,0:3].dot(m1[0:3])) 20 | return m1 21 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/inverse_measurement.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | 3 | def inverse_measurement(m,z): 4 | c = cos(m[2]+z[1,:]) 5 | s = sin(m[2]+z[1,:]) 6 | 7 | return m[0:2].reshape((2,1)) + vstack((z[0,:]*s,-z[0,:]*c)) 8 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/jacobian_measurement.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | 3 | from pdb import set_trace as bp 4 | 5 | def jacobian_measurement(m,ind): 6 | N = ind.size 7 | ind1 = 1+2*ind 8 | 9 | x = m[ind1] 10 | y = m[ind1+1] 11 | 12 | delta = vstack((x,y))-m[0:2].reshape((2,1)) 13 | q = sum(delta**2,axis=0) 14 | 15 | J = array([[1,0],[0,-1],[0,1],[1,0]]).dot(delta) 16 | #ugly 17 | J = vstack((vstack((vstack((-J,zeros([1,N]))),-ones([1,N]))),J)) 18 | J[0::2,:] = J[0::2,:] *1/sqrt(q) 19 | J[array([1,3,7,9]),:] = J[array([1,3,7,9]),:] *1/q 20 | 21 | return J.reshape((2,5,N), order='F') 22 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/jacobian_motion.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | 3 | def jacobian_motion(theta,v,a,dt): 4 | H = 0.74 5 | L = 2.83 6 | a1 = 0.95 + L 7 | b1 = 0.5 8 | 9 | return array([[1,0,-dt*(v*sin(theta)+v/L*tan(a)*(a1*cos(theta)-b1*sin(theta)))], \ 10 | [0,1,dt*(v*cos(theta)-v/L*tan(a)*(a1*sin(theta)+b1*cos(theta)))], \ 11 | [0,0,1]]) 12 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/markov_blanket.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | 3 | from pdb import set_trace as bp 4 | 5 | def markov_blanket(n,m0,G): 6 | s = G.shape[0] 7 | mb1 = m0 8 | mb2 = G[n,:].nonzero()[1] 9 | 10 | mb12 = intersect_int(mb1,mb2,s) 11 | if mb12.size == 0: 12 | mb = bfs(n,G) 13 | mb2 = union_int(mb2,mb,s) 14 | 15 | mb2 = hstack(([0,n],mb2)) 16 | mb = union_int(mb1,mb2,s) 17 | return mb 18 | 19 | def union_int(m1,m2,s): 20 | x = zeros(s) 21 | y = zeros(s) 22 | x[m1] = 1 23 | y[m2] = 1 24 | return nonzero(x+y)[0] 25 | 26 | def intersect_int(m1,m2,s): 27 | x = zeros(s) 28 | y = zeros(s) 29 | x[m1] = 1 30 | y[m2] = 1 31 | return nonzero(x*y)[0] 32 | 33 | def bfs(n,G): 34 | s = G.shape[0] 35 | d = -ones(s,dtype=int) 36 | d[0] = 0 37 | q = zeros(s,dtype=int) 38 | i=0 39 | j=1 40 | q[0]=0 41 | found=False 42 | p=zeros(0) 43 | curr=0 44 | while (not found) and i0: 19 | nsize = xi.size+2*new 20 | omega1 = zeros([nsize,nsize]) 21 | omega1[0:xi.size,0:xi.size] = omega 22 | xi1 = zeros(nsize) 23 | xi1[0:xi.size] = xi 24 | m1 = zeros(nsize) 25 | m1[0:xi.size] = m 26 | m1[xi.size:] = inverse_measurement(m[0:3],z[:,c>n]).T.ravel() 27 | s = G.shape[0] 28 | G.resize((s+new,s+new)) 29 | else: 30 | omega1=omega 31 | xi1=xi 32 | m1=m 33 | 34 | J = jacobian_measurement(m1,c) 35 | zhat = equation_measurement(m1,c) 36 | 37 | for i in range(0,c.size): 38 | j = c[i] 39 | h = J[:,:,i] 40 | dz = z[:,i]-zhat[:,i] 41 | 42 | dz[1] = measure(dz[1]) 43 | dz = dz.reshape((2,1)) 44 | ind = hstack((r_[0:3],[2*j+1,2*j+2])).toarray().ravel() 45 | 46 | Q1 = inv(Q) 47 | xi1[ind] = xi1[ind]+ h.T@Q1@(dz+h@m1[ind].reshape((5,1))).flatten() 48 | omega1[ix_(ind,ind)] = omega1[ix_(ind,ind)] + h.T@Q1@h 49 | 50 | c = unique(c) 51 | row = zeros(c.size) 52 | col = c 53 | data = ones(c.size) 54 | nl = csr_matrix((data,(row,col)),shape=(G.shape[0],G.shape[0]),dtype=bool) 55 | G = G + nl + nl.T 56 | 57 | return xi1,omega1,m1,G 58 | 59 | 60 | def measure(theta): 61 | tmp = theta%(2*pi) 62 | return tmp-minimum(floor(tmp/pi),1)*2*pi 63 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/motion.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | from scipy.sparse import csr_matrix,csc_matrix,lil_matrix 3 | from scipy.linalg import inv,solve 4 | import numpy as np 5 | 6 | from jacobian_motion import jacobian_motion 7 | from equation_motion import equation_motion 8 | 9 | from pdb import set_trace as bp 10 | 11 | def motion(v,a,dt,m0,xi,omega,m,G): 12 | n = xi.size 13 | R = diag([8e-4,8e-4,1e-6]) 14 | J = jacobian_motion(m[2],v,a,dt) 15 | 16 | psi = inv(J)-eye(3) 17 | ind = 2*sort(m0) + 1 18 | ind = vstack((ind,ind+1)).flatten(order='F') 19 | ind = hstack((r_[0:3],ind)).astype(int) 20 | omega_s = omega[ix_(ind,r_[0:3])] 21 | psi1 = omega_s@psi 22 | 23 | lmbd = zeros([n,n]) 24 | lmbd[ix_(ind,r_[0:3])] = psi1;lmbd[ix_(r_[0:3],ind)]=lmbd[ix_(r_[0:3],ind)]+psi1.T 25 | lmbd[0:3,0:3] = lmbd[0:3,0:3] + psi.T@omega[0:3,0:3]@psi 26 | phi = omega + lmbd 27 | 28 | phiX = phi[ix_(r_[0:3],ind)] 29 | k=zeros([n,n]) 30 | k[ix_(ind,ind)] = phiX.T@inv(inv(R)+phi[0:3,0:3])@phiX 31 | omega1 = phi-k 32 | 33 | dx = equation_motion(m[2],v,a,dt) 34 | omega_s = omega1[ix_(ind,r_[0:3])] 35 | 36 | xi1 = xi 37 | xi1[ind] = xi1[ind] + omega_s@dx 38 | xi1[ind] = xi1[ind] + (lmbd-k)[ind,:]@m 39 | m1 = m 40 | m1[0:3] = m1[0:3] + dx 41 | 42 | if m0.size>0: 43 | s,t = meshgrid(m0,m0) 44 | s = s.flatten() 45 | t = t.flatten() 46 | edges = sort(np.vstack((s,t)),axis=0) 47 | edges = unique(edges,axis=1) #needs version >=1.13 48 | #edges = np.vstack({tuple(row) for row in edges.T}).T 49 | edges = edges[:,edges[0,:]-edges[1,:]!=0] 50 | if edges.shape[1]>0: 51 | sg = G.shape[0] 52 | 53 | nl = csr_matrix((ones(edges.shape[1]),(edges[0,:],edges[1,:])),shape=(sg,sg),dtype=bool) 54 | G = G + nl + nl.T 55 | 56 | return xi1,omega1,m1,G 57 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/seif.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | import scipy.linalg as la 3 | from scipy.sparse import csr_matrix, save_npz 4 | import scipy.io as sio 5 | import os 6 | import time as timechr 7 | 8 | import matplotlib.pyplot as plt 9 | from motion import motion 10 | from estimate import estimate 11 | from correspondence import correspondence 12 | from measurement import measurement 13 | from sparsification import sparsification 14 | 15 | from pdb import set_trace as bp 16 | 17 | def seif(): 18 | filename1 = 'z.mat' 19 | filename2 = 'aa3_dr.mat' 20 | filename3 = 'aa3_lsr2.mat' 21 | filename4 = 'c_5000.mat' 22 | include_dir = './data/' 23 | 24 | z_contents = sio.loadmat(include_dir +filename1) 25 | z = z_contents['z']; 26 | 27 | z_contents = sio.loadmat(include_dir +filename2) 28 | speed = z_contents['speed'].ravel(); 29 | steering = z_contents['steering'].ravel(); 30 | time = z_contents['time'].ravel(); 31 | 32 | z_contents = sio.loadmat(include_dir +filename3) 33 | timeLsr = z_contents['TLsr'].ravel(); 34 | L = size(timeLsr,0); 35 | 36 | z_contents = sio.loadmat(include_dir +filename4) 37 | corresp = z_contents['corresp'] 38 | Lc = size(corresp,0); 39 | 40 | del z_contents 41 | 42 | dt = 25e-3 43 | G = csr_matrix((1,1),dtype=bool) 44 | Q = diag([5.0,0.02]) 45 | m = zeros(3) 46 | xi = zeros(3) 47 | omega = 10e4*eye(3) 48 | m0=zeros(0) 49 | 50 | #max active landmarks 51 | N = 20 52 | plt.ion() 53 | 54 | fig,ax = plt.subplots(1,1) 55 | ax.set_aspect('equal') 56 | ax.set_xlim(-100, 300) 57 | ax.set_ylim(-50, 350) 58 | ax.hold(True) 59 | plt.show(False) 60 | plt.draw() 61 | #background = fig.canvas.copy_from_bbox(ax.bbox) 62 | line1 = ax.plot(0,0,'b-')[0] 63 | line2 = ax.plot(-1000,1000,'ro',markersize=2)[0] 64 | 65 | poses = zeros([3,5000]) 66 | stindex=0 67 | j=searchsorted(timeLsr,time[stindex]) 68 | timechr.sleep(3) 69 | t1 = timechr.time() 70 | 71 | for i in range(stindex, time.shape[0]): 72 | #for i in range(stindex, 10000): 73 | t3= timechr.time() 74 | 75 | if i>stindex and i%5000==0: 76 | save(include_dir +'poses_'+str(i),poses) 77 | save(include_dir +'landmarks_'+str(i),m) 78 | save(include_dir +'xi_'+str(i),xi) 79 | save(include_dir +'omega_'+str(i),omega) 80 | save(include_dir +'m0_'+str(i),m0) 81 | save_npz(include_dir +'G_', G) 82 | 83 | xi,omega,m,G = motion(speed[i],steering[i],dt,m0,xi,omega,m,G) 84 | 85 | m = estimate(m0,xi,omega,m) 86 | 87 | while j0: 91 | co = correspondence(z1,m0,omega,m,G) 92 | xi,omega,m,G = measurement(z1,co,xi,omega,m,G) 93 | j+=1 94 | 95 | if co.size>0: 96 | n=(xi.size-3)//2 97 | m0a = setdiff1d(m0,co) 98 | #returns float when m0 empty 99 | tq = hstack((m0a,unique(co))).astype(int) 100 | tq = tq[max(0,tq.size-N):] 101 | m1 = setdiff1d(m0,tq) 102 | m1 = union1d(m1,setdiff1d(co,tq)) 103 | m0 = tq 104 | if m1.size>0: 105 | xi,omega,G = sparsification(m0,m1,xi,omega,m,G) 106 | 107 | print("\x1b[2J\x1b[H") 108 | t2=timechr.time() 109 | print('iter: '+str(i)) 110 | print('iter time: {0:.5f}'.format(t2-t3)) 111 | print('avg: {0:.5f}'.format((t2-t1)/(i+1))) 112 | poses[:,i%5000]=m[0:3] 113 | 114 | if i%5000==4999: 115 | line2.set_data(m[3::2],m[4::2]) 116 | line1.set_xdata(hstack((line1.get_xdata(),poses[0,:]))) 117 | line1.set_ydata(hstack((line1.get_ydata(),poses[1,:]))) 118 | #fig.canvas.restore_region(background) 119 | #ax.draw_artist(line1) 120 | #ax.draw_artist(line2) 121 | fig.canvas.blit(ax.bbox) 122 | fig.canvas.draw() 123 | #plt.show() 124 | #fig.canvas.flush_events() 125 | 126 | 127 | if __name__ == "__main__": 128 | # execute only if run as a script 129 | import cProfile 130 | pr = cProfile.Profile() 131 | pr.enable() 132 | seif() 133 | pr.disable() 134 | pr.print_stats(sort='cumtime') 135 | -------------------------------------------------------------------------------- /ch12_the_sparse_extended_information_filter/src/python/sparsification.py: -------------------------------------------------------------------------------- 1 | from numpy import * 2 | from scipy.sparse import csr_matrix 3 | from scipy.linalg import solve 4 | 5 | from pdb import set_trace as bp 6 | 7 | def sparsification(m0,m1,xi,omega,m,G): 8 | sg = G.shape[0] 9 | ol = csr_matrix((ones(m1.size),(zeros(m1.size),m1)),shape=(sg,sg),dtype=bool) 10 | G = G - ol - ol.T 11 | G = G.astype(bool) 12 | s,t = meshgrid(union1d(m0,m1),m1) 13 | s = s.flatten() 14 | t = t.flatten() 15 | edges = sort(vstack((s,t)),axis=0) 16 | edges = unique(edges,axis=1) #needs version >=1.13 17 | #edges = vstack({tuple(row) for row in edges.T}).T 18 | edges = edges[:,edges[0,:]-edges[1,:]!=0] 19 | nl = csr_matrix((ones(edges.shape[1]),(edges[0,:],edges[1,:])),shape=(sg,sg),dtype=bool) 20 | G = G + nl + nl.T 21 | 22 | m0 = sort(m0) 23 | m1 = sort(m1) 24 | m0 = 2*m0 + 1 25 | m0 = vstack((m0,m0+1)).flatten(order='F') 26 | m1 = 2*m1 + 1 27 | m1 = vstack((m1,m1+1)).flatten(order='F') 28 | 29 | omega2 = omega[ix_(hstack((hstack((r_[0:3],m0)),m1)),m1)] 30 | l1 = omega2.dot(solve(omega[ix_(m1,m1)],omega2.T)) 31 | 32 | omega3 = omega[ix_(hstack((hstack((r_[0:3],m0)),m1)),hstack((r_[0:3],m1)))] 33 | l2 = omega3.dot(solve(omega[ix_(hstack((r_[0:3],m1)),hstack((r_[0:3],m1)))],omega3.T)) 34 | 35 | omega4 = omega[:,r_[0:3]] 36 | l3 = omega4.dot(solve(omega[ix_(r_[0:3],r_[0:3])],omega4.T)) 37 | 38 | omega1 = copy(omega) 39 | omega1[ix_(hstack((hstack((r_[0:3],m0)),m1)),hstack((hstack((r_[0:3],m0)),m1)))] -= l1 40 | omega1[ix_(hstack((hstack((r_[0:3],m0)),m1)),hstack((hstack((r_[0:3],m0)),m1)))] += l2 41 | omega1 -= l3 42 | 43 | omega1[ix_(r_[0:3],m1)] = 0 44 | omega1[ix_(m1,r_[0:3])] = 0 45 | 46 | xi1 = xi + (omega1-omega)@m 47 | 48 | return xi1,omega1,G 49 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:18.04 2 | 3 | RUN apt-get update && apt-get install -y --no-install-recommends \ 4 | wget \ 5 | gnupg \ 6 | ca-certificates \ 7 | build-essential \ 8 | curl \ 9 | python-dev \ 10 | cmake 11 | 12 | RUN wget https://support.hdfgroup.org/ftp/HDF5/releases/hdf5-1.10/hdf5-1.10.1/src/CMake-hdf5-1.10.1.tar.gz \ 13 | && tar -xvf CMake-hdf5-1.10.1.tar.gz \ 14 | && cd CMake-hdf5-1.10.1\ 15 | && mkdir build \ 16 | && cd build \ 17 | && cmake ../hdf5-1.10.1 \ 18 | && ../hdf5-1.10.1/configure --enable-cxx --enable-shared --prefix=/usr \ 19 | && make -j -l6 \ 20 | && make install 21 | 22 | RUN curl -sL https://deb.nodesource.com/setup_10.x | bash - 23 | RUN apt install -y --no-install-recommends nodejs 24 | RUN wget --no-check-certificate https://apt.repos.intel.com/intel-gpg-keys/GPG-PUB-KEY-INTEL-SW-PRODUCTS-2019.PUB 25 | RUN apt-key add GPG-PUB-KEY-INTEL-SW-PRODUCTS-2019.PUB 26 | RUN sh -c 'echo deb https://apt.repos.intel.com/mkl all main > /etc/apt/sources.list.d/intel-mkl.list' 27 | RUN apt-get update 28 | RUN apt-get install -y --no-install-recommends intel-mkl-2018.2-046 29 | RUN rm -rf /var/lib/apt/lists/* 30 | 31 | RUN wget https://sourceforge.net/projects/arma/files/armadillo-9.200.7.tar.xz \ 32 | && tar -xvf armadillo-9.200.7.tar.xz \ 33 | && cd armadillo-9.200.7 \ 34 | && cmake . \ 35 | && ./configure \ 36 | && make \ 37 | && make install 38 | 39 | WORKDIR /app 40 | COPY . /app 41 | 42 | RUN ["make", "build" ] 43 | ENV LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/intel/compilers_and_libraries/linux/mkl/lib/intel64_lin:/opt/intel/compilers_and_libraries/linux/lib/intel64:/usr/local/lib64:/usr/local/lib:/usr/lib:/app/LocalInstall/lib 44 | 45 | ENTRYPOINT ["./fslam.bin"] 46 | CMD ["100"] 47 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/README.md: -------------------------------------------------------------------------------- 1 | # fastslam cpp implementation 2 | 3 | Implementation of the fastslam algorithm. You can execute natively after installing locally all the dependencies, or execute it in docker container. 4 | 5 | ## Building the Docker image and executing in container 6 | 7 | 1. Install [Docker](https://www.docker.com/get-started) on your machine. 8 | 9 | 2. Clone the repository 10 | 11 | ``` 12 | git clone https://github.com/pptacher/probabilistic_robotics.git 13 | ``` 14 | 15 | 3. Go to the source folder 16 | 17 | ``` 18 | cd probabilistic_robotics/ch13_the_fastslam_algorithm/src/cpp 19 | ``` 20 | 21 | 4. Build the docker image 22 | 23 | ``` 24 | docker build --tag=fastslam . 25 | ``` 26 | 27 | 5. Run the container; you can specify a number of particles (default to 100). 28 | 29 | ``` 30 | docker run fastslam 150 31 | ``` 32 | 33 | You can access the container id with 34 | ``` 35 | $ docker container ls 36 | CONTAINER ID IMAGE COMMAND CREATED STATUS PORTS NAMES 37 | ca6183eab770 fastslam "./fslam.bin" About a minute ago Up About a minute unruffled_hellman 38 | ``` 39 | copy back the output locally to your machine 40 | ``` 41 | $ docker cp e31da5e0537b:/app/data/poses.dat . 42 | ``` 43 | 44 | pause the container 45 | ``` 46 | docker pause ca6183eab770 47 | ``` 48 | 49 | stop the container 50 | ``` 51 | docker stop ca6183eab770 52 | ``` 53 | 54 | following command will remove all stopped containers, all dangling images, and all unused networks: 55 | ``` 56 | docker system prune 57 | ``` 58 | 59 | See also the [Docker cheat sheet](https://www.docker.com/sites/default/files/Docker_CheatSheet_08.09.2016_0.pdf). 60 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/btree.h: -------------------------------------------------------------------------------- 1 | #define ARMA_USE_HDF5 2 | //#define ARMA_USE_SUPERLU 1 3 | #define ARMA_64BIT_WORD 4 | #define DARMA_DONT_USE_WRAPPER 5 | 6 | #include 7 | #include 8 | //#include 9 | //#include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "mkl_lapacke.h" 18 | //#include "mkl.h" 19 | 20 | ///////////////////////////////////////////////////////////////////// 21 | //gdb helpers 22 | template 23 | void print_matrix(Matrix matrix) { 24 | matrix.print(std::cout); 25 | } 26 | 27 | 28 | template void print_matrix(arma::mat matrix); 29 | template void print_matrix(arma::cx_mat matrix); 30 | template void print_matrix>(arma::Col); 31 | template void print_matrix(arma::uvec); 32 | template void print_matrix(arma::rowvec); 33 | template void print_matrix(arma::vec); 34 | ///////////////////////////////////////////////////////////////////// 35 | 36 | 37 | #include 38 | #include 39 | 40 | using namespace arma; 41 | 42 | struct NodeRef { 43 | vec m; 44 | mat s; 45 | bool pflag = false; 46 | 47 | NodeRef(){} 48 | 49 | NodeRef(vec from_m,mat from_s,bool from_pflag): 50 | m(from_m), 51 | s(from_s), 52 | pflag(from_pflag){} 53 | }; 54 | 55 | class BTree{ 56 | 57 | class BTreeNode; 58 | class BTreeNodeI; 59 | class GaussianNode; 60 | 61 | public: 62 | enum direction : unsigned char {left, right}; 63 | 64 | //void initialize(vec,mat); 65 | bool add_node(uint,vec,mat); 66 | bool add_node(vec,mat); 67 | bool rm_node(uint); 68 | bool set_node(uint,vec,mat); 69 | bool get_node(uint,vec&,mat&) const; 70 | uint get_size() const; 71 | void print(std::ostream&) const; 72 | void select(vec,std::map&) const; 73 | bool dec_pcount(uint); 74 | void print_tree(std::ostream& os) const; 75 | 76 | private: 77 | uint height = 0; 78 | uint size = 0; 79 | static signed char const ptresholdl = -10; 80 | //static signed char const ptresholdh = 20; 81 | std::queue avail; 82 | //std::map pcounts; 83 | std::vector pcounts; 84 | std::shared_ptr root = nullptr; 85 | std::shared_ptr build_tree(uint,uint,vec&,mat&/*, signed char pc=0*/); 86 | void preorder(BTreeNode* p,std::ostream&) const; 87 | void select_rec(BTreeNode*,vec,std::map&, uint,uint&) const; 88 | void print_tree_rec(std::ostream& os,BTreeNode* p, uint h) const; 89 | 90 | }; 91 | 92 | class BTree::BTreeNode { 93 | public: 94 | virtual BTreeNode* next(direction) const= 0; 95 | virtual std::shared_ptr next_s(direction) const = 0; 96 | virtual void set_next(direction,std::shared_ptr) = 0; 97 | virtual void print(std::ostream&) const = 0; 98 | virtual bool get_μΣ(vec&,mat&) const = 0; 99 | //virtual float dec_pcount(uint) = 0; 100 | //virtual float get_pcount() const = 0; 101 | }; 102 | 103 | struct BTree::BTreeNodeI : public BTree::BTreeNode{ 104 | 105 | BTreeNodeI(std::shared_ptr left, std::shared_ptr right){ 106 | successors[0] = std::move(left); 107 | successors[1] = std::move(right); 108 | //std::cout << "ctor BTreeNodeI" << '\n'; 109 | } 110 | 111 | /*BTreeNodeI( const BTreeNodeI& from){ 112 | successors[0] = from.successors[0]; 113 | successors[1] = from.successors[1]; 114 | std::cout << "cpy ctor BTreeNodeI" << '\n'; 115 | }*/ 116 | 117 | BTreeNodeI(){ 118 | std::cout << "dflt ctor BTreeNodeI" << '\n'; 119 | } 120 | 121 | ~BTreeNodeI(){ 122 | //std::cout << "dtor BTreeNodeI" << '\n'; 123 | } 124 | 125 | //private: 126 | public: 127 | std::shared_ptr successors[2]; 128 | 129 | public: 130 | BTreeNode* next(direction d)const override{ 131 | //std::cout << "BTreeNodeI next" << '\n'; 132 | return successors[d].get(); 133 | } 134 | 135 | virtual std::shared_ptr next_s(direction d) const override{ 136 | //std::cout << "BTreeNodeI next_s" << '\n'; 137 | return successors[d]; 138 | } 139 | 140 | void set_next(direction d, std::shared_ptr ptr) override{ 141 | successors[d].reset(); 142 | successors[d] = std::move(ptr); 143 | } 144 | 145 | void print(std::ostream& os) const override{ 146 | os << "left" 147 | << " get() = " << successors[0].get() 148 | << " use_count() = " << successors[0].use_count() << '\n' ; 149 | os << "right" 150 | << " get() = " << successors[1].get() 151 | << " use_count() = " << successors[1].use_count() << '\n' ; 152 | } 153 | 154 | virtual bool get_μΣ(vec& m,mat& s) const override { 155 | return false; 156 | } 157 | 158 | /* virtual float dec_pcount(uint) override { 159 | return 0; 160 | }; 161 | 162 | virtual float get_pcount() const override { 163 | return 0; 164 | };*/ 165 | }; 166 | 167 | class BTree::GaussianNode :public BTree::BTreeNode{ 168 | public: 169 | GaussianNode(vec m,mat s/*, float pc = 0*/) 170 | /*pcount(pc) */{ 171 | this->μ = m; 172 | this->Σ = s; 173 | //std::cout << "ctor GaussianNode" << '\n'; 174 | //μ.print("μ:"); 175 | //Σ.print("Σ:"); 176 | } 177 | ~GaussianNode(){ 178 | //std::cout << "dtor GaussianNode" << '\n'; 179 | //μ.print("μ:"); 180 | //Σ.print("Σ:"); 181 | } 182 | private: 183 | vec μ = zeros(2); 184 | mat Σ = zeros(2,2); 185 | //float pcount; 186 | //mutable std::mutex pcount_mutex; 187 | 188 | public: 189 | BTreeNode* next(direction) const override{ 190 | return nullptr; 191 | } 192 | 193 | virtual std::shared_ptr next_s(direction d) const override{ 194 | return nullptr; 195 | } 196 | 197 | void print(std::ostream& os) const override{ 198 | μ.print(os,"μ:"); 199 | Σ.print(os,"Σ:"); 200 | } 201 | 202 | virtual bool get_μΣ(vec& m,mat& s) const override { 203 | m = μ; 204 | s = Σ; 205 | return true; 206 | } 207 | 208 | void set_next(direction d, std::shared_ptr ptr) override{}; 209 | 210 | /*virtual float dec_pcount(uint nn) override { 211 | std::lock_guard guard(pcount_mutex); 212 | pcount -= 1/((float) nn); 213 | return pcount; 214 | } 215 | 216 | virtual float get_pcount() const override { 217 | std::lock_guard guard(pcount_mutex); 218 | return pcount; 219 | }*/ 220 | }; 221 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/data/aa3_dr.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch13_the_fastslam_algorithm/src/cpp/data/aa3_dr.h5 -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/data/aa3_lsr2.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch13_the_fastslam_algorithm/src/cpp/data/aa3_lsr2.h5 -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/data/measurement.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch13_the_fastslam_algorithm/src/cpp/data/measurement.h5 -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/data/timeLsr.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch13_the_fastslam_algorithm/src/cpp/data/timeLsr.bin -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/fastslam.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include "particle.h" 3 | 4 | void fastslam(uint n){ 5 | std::string filename1 = "aa3_lsr2.h5"; 6 | std::string filename2 = "aa3_dr.h5"; 7 | std::string filename3 = "measurement.h5"; 8 | std::string filename4 = "timeLsr.bin"; 9 | std::string include_dir = "./data/"; 10 | 11 | std::string filename5 = include_dir + "poses.dat"; 12 | std::ofstream ostrm(filename5, std::ios::binary); 13 | 14 | std::string filename6 = include_dir + "ldmarks.dat"; 15 | std::ofstream ostrm1(filename6, std::ios::binary); 16 | 17 | arma::mat speed, steering, time, measurmt; 18 | uvec timelsr; 19 | speed.load(arma::hdf5_name(include_dir + filename2, "speed")); 20 | steering.load(arma::hdf5_name(include_dir + filename2, "steering")); 21 | time.load(arma::hdf5_name(include_dir + filename2, "time")); 22 | timelsr.load(include_dir + filename4); 23 | measurmt.load(arma::hdf5_name(include_dir + filename3, "zz")); 24 | 25 | 26 | static float const dt = 25e-3; 27 | Particle particles(n); 28 | 29 | uint stindex = 0; 30 | uvec jvec = find(timelsr >= time(stindex),1,"first"); 31 | uint llsr = timelsr.n_elem; 32 | uint j = jvec(0); 33 | 34 | //mat::fixed<5000,2> poses = zeros(5000,2); 35 | //uint max_threads = mkl_get_max_threads(); 36 | //mkl_set_num_threads(max_threads); 37 | 38 | std::chrono::steady_clock::time_point time_; 39 | std::chrono::steady_clock::time_point start_time_; 40 | std::chrono::steady_clock::time_point start_iter_; 41 | float deltatime_(0.0f); 42 | float deltatime_iter_(0.0f); 43 | start_time_ = std::chrono::steady_clock::now(); 44 | 45 | for (size_t i = stindex; i < time.n_elem; i++) { 46 | start_iter_ = std::chrono::steady_clock::now(); 47 | 48 | mat z = zeros(2,0); 49 | while (j < llsr && timelsr(j) < time(i)+dt) { 50 | z = join_horiz(z,measurmt(uvec({2*j,2*j+1}),find(measurmt(2*j,span::all) != 0))); 51 | ++j; 52 | } 53 | 54 | particles.motion(speed(i),steering(i),dt,z); 55 | 56 | time_ = std::chrono::steady_clock::now(); 57 | deltatime_ = std::chrono::duration_cast>(time_-start_time_).count(); 58 | deltatime_iter_ = std::chrono::duration_cast>(time_-start_iter_).count(); 59 | std::cout << "iter: " << std::setw(10) << i 60 | << std::setw(15) << "iter time: " << std::setw(10) << deltatime_iter_ 61 | << std::setw(15) << "avg: " << std::setw(10) << deltatime_/(i+1) << 62 | '\r' ; 63 | 64 | ostrm << particles << '\n'; 65 | 66 | if (i%1000==0) { 67 | ostrm1.seekp(0,std::ios::beg); 68 | particles.print_tree(ostrm1); 69 | } 70 | 71 | } 72 | ostrm1.seekp(0,std::ios::beg); 73 | particles.print_tree(ostrm1); 74 | 75 | 76 | std::cout << '\n'; 77 | } 78 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/linear_model.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace arma; 4 | 5 | const static double π = 3.141592653589793238463; 6 | const static float h = 0.74; 7 | const static float l = 2.83; 8 | const static float a = 0.95 + l; 9 | const static float b = 0.50; 10 | 11 | double measure(double θ){ 12 | int ϵ = (std::sin(θ) >=0) ? 1 : -1; 13 | double res = ϵ*std::acos(std::cos(θ)); 14 | if (res >= π || res < -π){ 15 | std::cout << ":()" << '\n'; 16 | } 17 | return res; 18 | } 19 | 20 | mat inverse(mat q){ 21 | double d = q(0,0)*q(1,1)-q(0,1)*q(1,0); 22 | return {{q(1,1)/d,-q(0,1)/d},{-q(1,0)/d,q(0,0)/d}}; 23 | } 24 | 25 | mat jacobian_motion(float θ, float v, float α, float dt){ 26 | return {{1, 0, -dt*(v*std::sin(θ)+v/l*std::tan(α)*(a*std::cos(θ)-b*std::sin(θ)))}, 27 | {0, 1, dt*(v*std::cos(θ)-v/l*std::tan(α)*(a*std::sin(θ)+b*std::cos(θ)))}, 28 | {0, 0, 1}}; 29 | } 30 | 31 | vec equation_motion(float θ, float v1, float α, float dt){ 32 | float v = v1/(1-std::tan(α)*h/l); 33 | return {dt*(v*std::cos(θ)-v/l*std::tan(α)*(a*std::sin(θ)+b*std::cos(θ))), 34 | dt*(v*std::sin(θ)+v/l*std::tan(α)*(a*std::cos(θ)-b*std::sin(θ))), 35 | dt*v/l*std::tan(α)}; 36 | } 37 | 38 | mat jacobian_measurement(vec μ, vec m){ 39 | //pb converting Col to uvec = Col 40 | vec δ = m-μ(span(0,1)); 41 | vec δ1 = sum(square(δ),0); 42 | double q = δ1(0); 43 | 44 | mat jcb = {{-δ(0),-δ(1),0,δ(0),δ(1)}, 45 | {δ(1),-δ(0),-q,-δ(1),δ(0)}}; 46 | jcb.row(0) *= (1/sqrt(q)); 47 | jcb.row(1) *= (1/q); 48 | 49 | return jcb; 50 | } 51 | 52 | vec equation_measurement(vec μ, vec m){ 53 | 54 | vec δ = m-μ(span(0,1)); 55 | vec δ1 = sqrt(sum(square(δ),0)); 56 | double q = δ1(0); 57 | double ϕ = atan2(δ(1),δ(0)) - μ(2) + π/2; 58 | return {q,ϕ}; 59 | } 60 | 61 | mat inverse_measurement(vec μ, mat z){ 62 | rowvec c = cos(z.row(1)+μ(2)); 63 | rowvec s = sin(z.row(1)+μ(2)); 64 | 65 | return mat(join_vert(z.row(0)%s,-z.row(0)%c)).each_col() + μ(span(0,1)); 66 | } 67 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/main.cc: -------------------------------------------------------------------------------- 1 | #include 2 | void fastslam(uint); 3 | 4 | int main(int argc, char** argv){ 5 | uint i=100; 6 | if (argc > 1) { 7 | i = (uint) std::stoi(argv[1]); 8 | } 9 | 10 | fastslam(i); 11 | 12 | return 0; 13 | } 14 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/makefile: -------------------------------------------------------------------------------- 1 | help: 2 | @echo 3 | @echo "make lib <-- Compile lib." 4 | @echo "make build <-- Compile all." 5 | @echo "make clean <-- Clean up." 6 | @echo "make install <-- Compile and install" 7 | @echo "make dist <-- Create .tar.gz project file" 8 | @echo 9 | 10 | PROJECT_NAME = fastslam 11 | VERSION = 001 12 | 13 | ARCHIVE_DIR_NAME = $(PROJECT_NAME)-$(VERSION) 14 | ARCHIVE_NAME = $(ARCHIVE_DIR_NAME).tar.gz 15 | 16 | FILES_TO_BE_ARCHIVED = *.h *.cc Makefile 17 | 18 | INCLUDE_PATH = -I. -I/opt/intel/compilers_and_libraries/linux/mkl/include/ -I/usr/include/hdf5/serial 19 | #LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib64:. 20 | COMPILO = g++ 21 | CXXFLAGS = $(RELEASEFLAGS) -m64 -Wall -Wextra -Wshadow -Werror -pedantic $(INCLUDE_PATH) -Wno-deprecated-declarations -Wno-unused-variable -Wno-unused 22 | CXXFLAGS_SHARED = $(CXXFLAGS) -fPIC -shared 23 | LIBS = -L. -L/usr/local/hdf5/lib/ -lfslam -larmadillo -lhdf5_cpp 24 | 25 | INSTALL_PREFIX = ./LocalInstall 26 | 27 | INSTALL_BIN_DIR = $(INSTALL_PREFIX)/bin 28 | INSTALL_LIB_DIR = $(INSTALL_PREFIX)/lib 29 | INSTALL_INCLUDE_DIR = $(INSTALL_PREFIX)/include/seif 30 | 31 | INSTALL_DIRS = \ 32 | $(INSTALL_BIN_DIR) \ 33 | $(INSTALL_LIB_DIR) \ 34 | $(INSTALL_INCLUDE_DIR) 35 | 36 | ########################################################## 37 | #patch for gcc: pre process to convert UTF-8 encoding to Unicode code point. 38 | #ref: https://gcc.gnu.org/wiki/FAQ#What_is_the_status_of_adding_the_UTF-8_support_for_identifier_names_in_GCC.3F 39 | PROCESS_UTF8 = ./to_UCN.sh 40 | 41 | #SHELL := /bin/sh 42 | #CC := g++ 43 | #CXX := g++ 44 | 45 | #memos 46 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/intel/compilers_and_libraries/linux/mkl/lib/intel64_lin:/opt/intel/compilers_and_libraries/linux/lib/intel64:/usr/local/lib64:/usr/local/lib:. 47 | export LIBRARY_PATH=/opt/intel/compilers_and_libraries/linux/mkl/lib/intel64_lin:/opt/intel/compilers_and_libraries/linux/lib/intel64 48 | #export MKL_VERBOSE=1 49 | #gnuplot 50 | #plot "poses.dat" using 91:92 pt 7 ps 0.1 lc rgb "black", "ldmarks.dat" using 1:2 pt 7 ps 0.6 lc rgb "green" 51 | 52 | COMPILER_OPTIONS := -m64 -Wall -Wextra -Wshadow -Werror -pedantic -Iinclude -Wno-unused-parameter -Wno-unused-variable -Wno-deprecated-declarations 53 | #INCLUDE_PATH = -I. 54 | 55 | #CXX_DEFINES = -DSHADERS_DIR=\"/Users/TACHER/sparklepp/src/shaders\" -DUSE_GLEW 56 | 57 | #CFLAGS := -std=c11 $(COMPILER_OPTIONS) 58 | #CXXFLAGS := -std=c++14 -Weffc++ $(COMPILER_OPTIONS) $(INCLUDE_PATH) 59 | 60 | DEBUGFLAGS := -g -O0 -D _DEBUG 61 | RELEASEFLAGS := -O2 -D NDEBUG 62 | ########################################################## 63 | 64 | # Fabrication des .o (lib) 65 | 66 | particle.o : particle.cc 67 | $(PROCESS_UTF8) particle.h 68 | $(PROCESS_UTF8) btree.h 69 | $(PROCESS_UTF8) particle.cc 70 | $(COMPILO) -c -std=c++17 $(CXXFLAGS_SHARED) /tmp/particle.cc 71 | 72 | fastslam.o : fastslam.cc 73 | $(PROCESS_UTF8) particle.h 74 | $(PROCESS_UTF8) btree.h 75 | $(PROCESS_UTF8) fastslam.cc 76 | $(COMPILO) -c -std=c++17 $(CXXFLAGS_SHARED) /tmp/fastslam.cc 77 | 78 | linear_model.o : linear_model.cc 79 | $(PROCESS_UTF8) linear_model.cc 80 | $(COMPILO) -c -std=c++17 $(CXXFLAGS_SHARED) /tmp/linear_model.cc 81 | 82 | # Fabrication des .o (hors lib) 83 | 84 | main.o : main.cc 85 | $(COMPILO) -c -std=c++17 $(CXXFLAGS) main.cc 86 | 87 | main0.o : main0.cc 88 | $(PROCESS_UTF8) particle.h 89 | $(PROCESS_UTF8) main0.cc 90 | $(COMPILO) -c -std=c++17 $(CXXFLAGS) /tmp/main0.cc 91 | 92 | # Fabrication de la lib 93 | 94 | libfslam.so : particle.o fastslam.o linear_model.o 95 | $(COMPILO) -o libfslam.so -shared -larmadillo particle.o fastslam.o linear_model.o 96 | 97 | # Fabrication de l'exécutable 98 | 99 | fslam.bin : main.o libfslam.so 100 | $(COMPILO) -o fslam.bin main.o $(LIBS) -lpthread -lmkl_core -lmkl_def -lmkl_intel_thread -lmkl_core -liomp5 -lm -ldl 101 | #-DARMA_DONT_USE_WRAPPER -lmkl_intel_thread -lmkl_intel_ilp64 102 | test.bin : main0.o libfslam.so 103 | $(COMPILO) -o test.bin main0.o $(LIBS) -lpthread -lmkl_core -lmkl_def -lmkl_intel_thread -lmkl_core -liomp5 -lm -ldl 104 | # Les cibles de l'aide 105 | 106 | lib : libfslam.so 107 | 108 | build : fslam.bin 109 | 110 | clean : 111 | @rm data/poses.dat data/ldmarks.dat 112 | @rm -f *~ *.o *.so *.bin 113 | @rm -rf $(ARCHIVE_DIR_NAME) $(ARCHIVE_NAME) 114 | @echo 115 | @echo "Cleaned up." 116 | 117 | install : fslam.bin libfslam.so 118 | @mkdir -p $(INSTALL_DIRS) 119 | @cp fslam.bin $(INSTALL_BIN_DIR) 120 | @cp data -r $(INSTALL_BIN_DIR) 121 | @cp libfslam.so $(INSTALL_LIB_DIR) 122 | @cp *.h $(INSTALL_INCLUDE_DIR) 123 | @echo "File installed in " $(INSTALL_BIN_DIR) $(INSTALL_LIB_DIR) $(INSTALL_INCLUDE_DIR) 124 | 125 | dist : 126 | @rm -rf $(ARCHIVE_DIR_NAME) $(ARCHIVE_NAME) 127 | @mkdir $(ARCHIVE_DIR_NAME) 128 | @cp $(FILES_TO_BE_ARCHIVED) $(ARCHIVE_DIR_NAME) 129 | @tar zcf $(ARCHIVE_NAME) $(ARCHIVE_DIR_NAME) 130 | @rm -rf $(ARCHIVE_DIR_NAME) 131 | @echo "File" $(ARCHIVE_NAME) " generated." 132 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/model.h: -------------------------------------------------------------------------------- 1 | 2 | vec equation_motion(float, float, float, float); 3 | mat jacobian_measurement(vec, vec); 4 | mat equation_measurement(vec, vec); 5 | mat inverse_measurement(vec, mat); 6 | 7 | double measure(double); 8 | mat inverse(mat q); 9 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/particle.h: -------------------------------------------------------------------------------- 1 | #define ARMA_USE_HDF5 2 | //#define ARMA_USE_SUPERLU 1 3 | #define ARMA_64BIT_WORD 4 | #define DARMA_DONT_USE_WRAPPER 5 | 6 | #include "btree.h" 7 | 8 | class Particle{ 9 | 10 | public: 11 | Particle(): 12 | particleCount(100), 13 | position(3,100,fill::zeros), 14 | weight(100,fill::ones) 15 | {b_trees = new BTree[100];} 16 | 17 | Particle(uint n): 18 | particleCount(n), 19 | position(3,n,fill::zeros), 20 | weight(n,fill::ones) 21 | {b_trees = new BTree[n];} 22 | 23 | ~Particle(){ 24 | delete[] b_trees; 25 | } 26 | 27 | void motion(double,double,double,mat z); 28 | void resample(); 29 | void print(std::ostream&) const; 30 | 31 | void print_tree(std::ostream& os) const{ 32 | b_trees[0].print_tree(os); 33 | } 34 | 35 | private: 36 | unsigned int const particleCount; 37 | mat position; 38 | vec weight; 39 | BTree* b_trees; 40 | 41 | void motion_measurement(double,double,double,mat); 42 | void resample_dist(BTree*,mat&,std::map&,uint*,uint,uint); 43 | 44 | }; 45 | 46 | std::ostream& operator<< (std::ostream&, const BTree&); 47 | std::ostream& operator<< (std::ostream&, const Particle&); 48 | -------------------------------------------------------------------------------- /ch13_the_fastslam_algorithm/src/cpp/to_UCN.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | cat $1 | perl -pe 'BEGIN { binmode STDIN, ":utf8"; } s/(.)/ord($1) < 128 ? $1 : sprintf("\\U%08x", ord($1))/ge;' > /tmp/$1 3 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/ch14_markov_decision_processes.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch14_markov_decision_processes/ch14_markov_decision_processes.pdf -------------------------------------------------------------------------------- /ch14_markov_decision_processes/ch14_markov_decision_processes.tex: -------------------------------------------------------------------------------- 1 | \documentclass[11pt, fleqn]{amsart} 2 | \usepackage[T1]{fontenc} 3 | %\usepackage[latin1]{inputenc} 4 | %\usepackage[english,francais]{babel} 5 | \usepackage[retainorgcmds]{IEEEtrantools} 6 | \usepackage{graphicx} 7 | \usepackage{amssymb} 8 | \usepackage{pst-slpe} 9 | \usepackage{auto-pst-pdf} 10 | \usepackage{pst-node} 11 | \usepackage{pst-plot} 12 | \usepackage{pst-3dplot} 13 | \usepackage{cancel} 14 | \usepackage{empheq} 15 | \usepackage{pst-grad,multido} 16 | \usepackage{graphicx,url,etoolbox} 17 | \usepackage{pifont} 18 | \usepackage{subfig} 19 | \usepackage{siunitx} 20 | \usepackage{mathtools} 21 | \usepackage{filecontents} 22 | \usepackage{amsthm} 23 | \usepackage{yhmath} 24 | \usepackage{enumitem} 25 | \usepackage{hyperref} 26 | \hypersetup{ 27 | colorlinks = true, 28 | linkbordercolor = {white}, 29 | linkcolor=blue 30 | } 31 | %\usepgfplotslibrary{external} 32 | \usepackage{standalone} 33 | %\tikzexternalize 34 | \DeclarePairedDelimiter\ceil{\lceil}{\rceil} 35 | \DeclarePairedDelimiter\floor{\lfloor}{\rfloor} 36 | \usepackage{pstricks-add} 37 | \let\clipbox\relax 38 | %\usepackage{pgfplots} 39 | \usepackage{stmaryrd} 40 | \usepackage{lipsum} 41 | %\usepackage{tikz} 42 | 43 | \setlength{\textwidth}{\paperwidth} 44 | \addtolength{\textwidth}{-2in} 45 | \calclayout 46 | 47 | \DeclareMathOperator{\vect}{vec} 48 | 49 | \graphicspath{{./include/}} 50 | \newcommand{\norm}[1]{\left\lVert#1\right\rVert} 51 | 52 | \setcounter{MaxMatrixCols}{20} 53 | 54 | \theoremstyle{definition} 55 | \newtheorem*{Lemma}{Lemma} 56 | \theoremstyle{definition} 57 | \newtheorem*{Theorem}{Theorem} 58 | \theoremstyle{definition} 59 | \newtheorem*{Definition}{Definition} 60 | \theoremstyle{definition} 61 | \newtheorem*{Proposition}{Proposition} 62 | \theoremstyle{remark} 63 | \newtheorem*{rmk}{Remark} 64 | 65 | \patchcmd{\section}{\scshape}{\bfseries}{}{} 66 | \makeatletter 67 | \renewcommand{\@secnumfont}{\bfseries} 68 | \makeatother 69 | \newcommand{\overbar}[1]{\mkern 1.5mu\overline{\mkern-1.5mu#1\mkern-1.5mu}\mkern 1.5mu} 70 | 71 | \newcommand{\Var}{\mathrm{Var}} 72 | \newcommand{\E}{\mathrm{E}} 73 | \newcommand{\Lip}{\mathrm{Lip}} 74 | \newcommand{\Cov}{\mathrm{Cov}} 75 | \renewcommand{\Re}{\operatorname{Re}} 76 | \renewcommand{\Im}{\operatorname{Im}} 77 | \newcommand{\atgt}{\mathrm{atan2}} 78 | \newcommand{\KL}{\mathrm{KL}} 79 | \newcommand{\id}{\mathrm{id}} 80 | \newcommand{\I}{\mathrm{I}} 81 | \newcommand{\p}{\mathrm{p}} 82 | 83 | \usepackage{stmaryrd} 84 | \usepackage{lipsum} 85 | \DeclareGraphicsRule{.tif}{png}{.png}{`convert #1 `dirname #1`/`basename #1 .tif`.png} 86 | 87 | \addtolength{\voffset}{-3cm} 88 | \addtolength{\textheight}{4cm} 89 | \newcommand{\ud}{\,\mathrm{d}} 90 | 91 | \makeatletter 92 | \patchcmd{\@settitle}{center}{flushleft}{}{} 93 | \patchcmd{\@settitle}{center}{flushleft}{}{} 94 | %\patchcmd{\@setauthors}{\centering}{\raggedright}{}{} 95 | \patchcmd{\abstract}{3pc}{0pt}{}{} % remove indentation 96 | \makeatother 97 | %\pgfplotsset{compat=1.16} 98 | 99 | \title{% 100 | Probabilistic Robotics: \\ 101 | Markov decision processes} 102 | 103 | \begin{document} 104 | \maketitle 105 | \author{Pierre-Paul TACHER} 106 | 107 | \setcounter{section}{1} 108 | \section{} There are planning algorithms like Lifelong Planning $A^{*}$ and Dynamic $A^{*}$ Lite ~\cite{likhachev1} or $D^{*}$ lite~\cite{likhachev2} which adresse the context of time varying cost function and the problem of replanning reusing the results of previous searches. 109 | 110 | \section{} 111 | \subsection{} In this section the actions are deterministic and the state can be described using only the position, using for instance convention in figure~\ref{0}. Note there is no need to use a discount factor $(\gamma=1)$ for future payoff in this setting. For the mathematically inclined reader, I give a proof of convergence of the value iteration algorithm in the stochastic shortest path setting (which encompass our simple example): cf.~\ref{ssp}. Algorithm is implemented in github repository. The quite obvious 2 equally optimal policies are represented in figure~\ref{1}. 112 | \begin{figure} 113 | \includegraphics[width=0.5\textwidth]{tikz1-figure0.pdf} 114 | \caption{Problem setting} 115 | \label{0} 116 | \end{figure} 117 | \begin{figure} 118 | \includegraphics[width=0.5\textwidth]{tikz1-figure1.pdf} 119 | \caption{Optimal policies} 120 | \label{1} 121 | \end{figure} 122 | \subsection{} Optimal policies does not change by introducing a bit of stochasticity in action. See figure~\ref{2} for optimal expected payoff starting from each position. 123 | \subsection{} We have to add a dimension to the state variable to handle the hidden state variable; there are 3 potential states regarding the knowledge of the position of the final reward. Thus we can modelize the state by 124 | \begin{IEEEeqnarray*}{rCl} 125 | x & = & (x_{1}, x_{2}) \in \mathbb{R}^{2} \\ 126 | \end{IEEEeqnarray*} 127 | where 128 | \begin{IEEEeqnarray*}{rCl} 129 | x_{1} & \in & \llbracket 0, 17 \rrbracket \\ 130 | \end{IEEEeqnarray*} 131 | \begin{equation*} 132 | x_{2} = \left\{ \, 133 | \begin{IEEEeqnarraybox}[][c]{l?s} 134 | \IEEEstrut 135 | -1 & if it is known the reward is on the left, \\ 136 | 0 & in absence of any a priori information, \\ 137 | +1 & if it is known the reward is on the right 138 | \IEEEstrut 139 | \end{IEEEeqnarraybox} 140 | \right. 141 | \end{equation*} 142 | The knowledge $x_{2}$ can change only when reaching position $X$ or ending game in either position $H$; the transition are represented in figure~\ref{3}. The expected maximum payoff is represented in figure~\ref{4}. The optimal policy is represented in figure~\ref{5}. 143 | \begin{figure} 144 | \includegraphics[width=0.5\textwidth]{tikz1-figure6.pdf} 145 | \includegraphics[width=0.5\textwidth]{tikz1-figure7.pdf} 146 | \caption{State values} 147 | \label{2} 148 | \end{figure} 149 | \begin{figure} 150 | \includegraphics[width=0.7\textwidth]{tikz1-figure2.pdf} 151 | \caption{Transition between $x_{2}=0$ and $x_{2}=\epsilon \in \{-1,1\}$} 152 | \label{3} 153 | \end{figure} 154 | \begin{figure} 155 | \includegraphics[width=0.7\textwidth]{tikz1-figure3.pdf} 156 | \includegraphics[width=0.7\textwidth]{tikz1-figure5.pdf} 157 | \caption{States values} 158 | \label{4} 159 | \end{figure} 160 | \begin{figure} 161 | \includegraphics[width=0.7\textwidth]{tikz1-figure4.pdf} 162 | \caption{Optimal policy} 163 | \label{5} 164 | \end{figure} 165 | 166 | \clearpage 167 | \appendix 168 | \section{} 169 | \subsection{} 170 | \label{ssp} 171 | \begin{Proposition} To do. 172 | \end{Proposition} 173 | \begin{proof} 174 | \end{proof} 175 | \clearpage 176 | {\small 177 | \begin{thebibliography}{99} 178 | \bibitem{likhachev1} Likhachev,~Maxim; Koenig,~Sven: 179 | \emph{Lifelong Planning A* and Dynamic A* Lite: The proofs}, 180 | (2001) 181 | \bibitem{likhachev2} Likhachev,~Maxim; Koenig,~Sven: 182 | \emph{D* lite}, 183 | American Association for Artificial Intelligence (2002) 184 | \end{thebibliography} 185 | } 186 | \end{document} -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/main.cc: -------------------------------------------------------------------------------- 1 | #include "markovdp.h" 2 | 3 | void move(const std::valarray&, const std::valarray&, std::map, double, valarray_comp>&,double&); 4 | void move_rdm(const std::valarray&, const std::valarray&, std::map, double, valarray_comp>&,double&); 5 | void warp(const std::valarray&, std::map, double, valarray_comp>&,double&); 6 | std::vector build_transitions(const mat&, 7 | std::vector&, std::map, double, valarray_comp>&,double&)>&); 8 | 9 | 10 | int main(int argc, char** argv){ 11 | 12 | mat occupancy; 13 | occupancy.load(arma::hdf5_name("occupancy_map.h5", "occupancy")); 14 | uint n = 0; 15 | for (size_t i = 0; i < occupancy.n_rows; i++) { 16 | for (size_t j = 0; j < occupancy.n_cols; j++) { 17 | if (occupancy(i,j) == 0) { 18 | occupancy(i,j) =n++; 19 | continue; 20 | } 21 | } 22 | } 23 | 24 | vec jvalues = zeros(54); 25 | //terminal states. 26 | jvalues(0) = +100; 27 | jvalues(1) = -100; 28 | jvalues(36) = -100; 29 | jvalues(37) = +100; 30 | 31 | std::vector&, std::map, double, valarray_comp>&,double&)> moves; 32 | 33 | moves.push_back([](const auto& from, auto& to, double& cost){ 34 | return move_rdm(std::valarray({0,1,0}),from,to,cost);}); 35 | moves.push_back([](const auto& from, auto& to, double& cost){ 36 | return move_rdm(std::valarray({1,0,0}),from,to,cost);}); 37 | moves.push_back([](const auto& from, auto& to, double& cost){ 38 | return move_rdm(std::valarray({0,-1,0}),from,to,cost);}); 39 | moves.push_back([](const auto& from, auto& to, double& cost){ 40 | return move_rdm(std::valarray({-1,0,0}),from,to,cost);}); 41 | moves.push_back(warp); 42 | 43 | std::vector transitions = build_transitions(occupancy, moves); 44 | 45 | MarkovDP mdp(jvalues, transitions); 46 | 47 | uvec policy(54); 48 | uint last_iter(0); 49 | double epsilon = 1e-2; 50 | uint max_iter = 1000; 51 | mdp.value_iteration(epsilon, max_iter, jvalues, policy, last_iter); 52 | 53 | std::cout << "converged in " << last_iter << " iterations." << '\n'; 54 | jvalues.print("Jvalues: "); 55 | policy.print("optimal policy: "); 56 | 57 | return 0; 58 | } 59 | 60 | void move(const std::valarray& dxy, const std::valarray& from, std::map, double, valarray_comp>& to,double& cost) { 61 | cost = -1.0; 62 | if (from[0] == 0 || ( from[0] == 7 && from[1] == 6 && from[2] == 0 )) { 63 | return; 64 | } 65 | to[from + dxy] = 1.0; 66 | } 67 | 68 | void move_rdm(const std::valarray& dxy, const std::valarray& from, std::map, double, valarray_comp>& to,double& cost) { 69 | cost = -1.0; 70 | if (from[0] == 0 || ( from[0] == 7 && from[1] == 6 && from[2] == 0 )) { 71 | return; 72 | } 73 | 74 | to[from + dxy] = 0.9; 75 | to[from - dxy] = 0.1/3; 76 | to[from + std::valarray({dxy[1],dxy[0],dxy[2]})] = 0.1/3; 77 | to[from - std::valarray({dxy[1],dxy[0],dxy[2]})] = 0.1/3; 78 | 79 | } 80 | 81 | void warp(const std::valarray& from, std::map, double, valarray_comp>& to,double& cost) { 82 | cost = 0.0; 83 | 84 | if (from[0] == 0 && from[2] == 0) { 85 | to[std::valarray({from[0], from[1], 1})] = 0.5; 86 | to[std::valarray({from[0], from[1], -1})] = 0.5; 87 | return; 88 | } 89 | 90 | if ( from[0] == 7 && from[1] == 6 && from[2] == 0) { 91 | to[std::valarray({7, 6, 1})] = 0.5; 92 | to[std::valarray({7, 6, -1})] = 0.5; 93 | return; 94 | } 95 | 96 | } 97 | 98 | inline uint to_linear(std::valarray coord, const mat& occupancy) { 99 | return (coord[2]+1)*18 + (uint)occupancy(coord[0],coord[1]); 100 | } 101 | 102 | std::vector build_transitions(const mat& occupancy, 103 | std::vector&, std::map, double, valarray_comp>&,double&)>& moves) { 104 | 105 | std::vector transitions; 106 | for (size_t i = 0; i < moves.size(); i++) { 107 | transitions.push_back({sp_mat(54,54),vec(54)}); 108 | } 109 | int nr = occupancy.n_rows; 110 | int nc = occupancy.n_cols; 111 | 112 | for (int i = 0; i < nr; i++) { 113 | for (int j = 0; j < nc; j++) { 114 | if (occupancy(i,j)==-1) { 115 | continue; 116 | } 117 | for (int z = -1; z < 2; z++) { 118 | std::valarray from({i, j, z}); 119 | uint lin_indx = to_linear(from, occupancy); 120 | for ( uint m=0; m < moves.size(); ++m) { 121 | 122 | std::map, double, valarray_comp> to; 123 | double cost; 124 | moves[m](from,to,cost); 125 | 126 | double stay(0); 127 | for( auto const& tovtx : to ) { 128 | auto const& tocoord = tovtx.first; 129 | auto const& toprob = tovtx.second; 130 | 131 | 132 | if (tocoord[0] < 0 || tocoord[0] >= nr || 133 | tocoord[1] < 0 || tocoord[1] >= nc || 134 | occupancy(tocoord[0], tocoord[1]) == -1) { 135 | stay += toprob; 136 | } 137 | else { 138 | uint lin_indx1 = to_linear(tocoord, occupancy); 139 | transitions[m].tst_proba(lin_indx,lin_indx1) = toprob; 140 | } 141 | } 142 | 143 | transitions[m].tst_proba(lin_indx,lin_indx) = (to.size())?stay:1; 144 | transitions[m].tst_cost(lin_indx) = (stay<1)?cost:0; 145 | 146 | } 147 | } 148 | } 149 | } 150 | 151 | return transitions; 152 | } 153 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/makefile: -------------------------------------------------------------------------------- 1 | help: 2 | @echo 3 | @echo "make lib <-- Compile lib." 4 | @echo "make build <-- Compile all." 5 | @echo "make clean <-- Clean up." 6 | @echo "make install <-- Compile and install" 7 | @echo "make dist <-- Create .tar.gz project file" 8 | @echo 9 | 10 | PROJECT_NAME = markovdp 11 | VERSION = 001 12 | 13 | ARCHIVE_DIR_NAME = $(PROJECT_NAME)-$(VERSION) 14 | ARCHIVE_NAME = $(ARCHIVE_DIR_NAME).tar.gz 15 | 16 | FILES_TO_BE_ARCHIVED = *.h *.cc Makefile 17 | 18 | INCLUDE_PATH = -I. -I/opt/intel/compilers_and_libraries/linux/mkl/include/ -I/usr/include/hdf5/serial 19 | LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib64:. 20 | COMPILO = g++ 21 | CXXFLAGS = $(DEBUGFLAGS) -m64 -Wall -Wextra -Wshadow -Werror -pedantic $(INCLUDE_PATH) -Wno-deprecated-declarations -Wno-unused-variable -Wno-unused 22 | CXXFLAGS_SHARED = $(CXXFLAGS) -fPIC -shared 23 | LIBS = -L. -L/usr/local/hdf5/lib -lmarkovdp -larmadillo -lhdf5_serial -lhdf5_cpp 24 | 25 | INSTALL_PREFIX = ./LocalInstall 26 | 27 | INSTALL_BIN_DIR = $(INSTALL_PREFIX)/bin 28 | INSTALL_LIB_DIR = $(INSTALL_PREFIX)/lib 29 | INSTALL_INCLUDE_DIR = $(INSTALL_PREFIX)/include/seif 30 | 31 | INSTALL_DIRS = \ 32 | $(INSTALL_BIN_DIR) \ 33 | $(INSTALL_LIB_DIR) \ 34 | $(INSTALL_INCLUDE_DIR) 35 | 36 | ########################################################## 37 | #patch for gcc: pre process to convert UTF-8 encoding to Unicode code point. 38 | #ref: https://gcc.gnu.org/wiki/FAQ#What_is_the_status_of_adding_the_UTF-8_support_for_identifier_names_in_GCC.3F 39 | PROCESS_UTF8 = ./to_UCN.sh 40 | 41 | #SHELL := /bin/sh 42 | #CC := g++ 43 | #CXX := g++ 44 | 45 | #memos 46 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/lib/x86_64-linux-gnu/hdf5/serial:/opt/intel/compilers_and_libraries/linux/mkl/lib/intel64_lin:/opt/intel/compilers_and_libraries/linux/lib/intel64:/usr/local/lib64:/usr/local/lib:. 47 | export LIBRARY_PATH=/opt/intel/compilers_and_libraries/linux/mkl/lib/intel64_lin:/opt/intel/compilers_and_libraries/linux/lib/intel64 48 | #export MKL_VERBOSE=1 49 | #gnuplot 50 | #plot "poses.dat" using 91:92 pt 7 ps 0.1 lc rgb "black", "ldmarks.dat" using 1:2 pt 7 ps 0.6 lc rgb "green" 51 | 52 | COMPILER_OPTIONS := -m64 -Wall -Wextra -Wshadow -Werror -pedantic -Iinclude -Wno-unused-parameter -Wno-unused-variable -Wno-deprecated-declarations 53 | #INCLUDE_PATH = -I. 54 | 55 | #CXX_DEFINES = -DSHADERS_DIR=\"/Users/TACHER/sparklepp/src/shaders\" -DUSE_GLEW 56 | 57 | #CFLAGS := -std=c11 $(COMPILER_OPTIONS) 58 | #CXXFLAGS := -std=c++14 -Weffc++ $(COMPILER_OPTIONS) $(INCLUDE_PATH) 59 | 60 | DEBUGFLAGS := -g -O0 -D _DEBUG 61 | RELEASEFLAGS := -O2 -D NDEBUG 62 | ########################################################## 63 | 64 | # Fabrication des .o (lib) 65 | 66 | 67 | # Fabrication des .o (hors lib) 68 | 69 | markovdp.o : markovdp.cc 70 | $(PROCESS_UTF8) markovdp.h 71 | $(PROCESS_UTF8) markovdp.cc 72 | $(COMPILO) -c -std=c++17 $(CXXFLAGS_SHARED) /tmp/markovdp.cc 73 | 74 | main.o : main.cc 75 | $(COMPILO) -c -std=c++17 $(CXXFLAGS) main.cc 76 | 77 | # Fabrication de la lib 78 | 79 | libmarkovdp.so : markovdp.o 80 | $(COMPILO) -o libmarkovdp.so -shared -larmadillo markovdp.o 81 | 82 | # Fabrication de l'exécutable 83 | 84 | vi.bin : main.o libmarkovdp.so 85 | $(COMPILO) -o vi.bin main.o $(LIBS) -lpthread -lmkl_core -lmkl_def -lmkl_intel_thread -lmkl_core -liomp5 -lm -ldl 86 | 87 | # Les cibles de l'aide 88 | 89 | lib : 90 | 91 | build : vi.bin 92 | 93 | clean : 94 | @rm -f *~ *.o *.so *.bin 95 | @rm -rf $(ARCHIVE_DIR_NAME) $(ARCHIVE_NAME) 96 | @echo 97 | @echo "Cleaned up." 98 | 99 | install : fslam.bin libfslam.so 100 | @mkdir -p $(INSTALL_DIRS) 101 | @cp vi.bin $(INSTALL_BIN_DIR) 102 | @cp *.h $(INSTALL_INCLUDE_DIR) 103 | @echo "File installed in " $(INSTALL_BIN_DIR) $(INSTALL_LIB_DIR) $(INSTALL_INCLUDE_DIR) 104 | 105 | dist : 106 | @rm -rf $(ARCHIVE_DIR_NAME) $(ARCHIVE_NAME) 107 | @mkdir $(ARCHIVE_DIR_NAME) 108 | @cp $(FILES_TO_BE_ARCHIVED) $(ARCHIVE_DIR_NAME) 109 | @tar zcf $(ARCHIVE_NAME) $(ARCHIVE_DIR_NAME) 110 | @rm -rf $(ARCHIVE_DIR_NAME) 111 | @echo "File" $(ARCHIVE_NAME) " generated." 112 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/markovdp.cc: -------------------------------------------------------------------------------- 1 | #include "markovdp.h" 2 | 3 | void MarkovDP::set_jvalues(vec jval) { 4 | jvalues = jval; 5 | } 6 | void MarkovDP::set_transitions(std::vector trst) { 7 | transitions = trst; 8 | } 9 | 10 | void MarkovDP::value_iteration(double ϵ, uint max_iter, vec& jval, uvec& policy, uint& last_iter) { 11 | bool converged = false; 12 | uint ns = jvalues.n_elem; 13 | uint i(0); 14 | 15 | while (!converged && (i < max_iter)) { 16 | mat jvalmat = zeros(ns,transitions.size()); 17 | 18 | for( uint j=0; j < transitions.size(); ++j ) { 19 | sp_mat const& tst_proba = transitions[j].tst_proba; 20 | vec const& tst_cost = transitions[j].tst_cost; 21 | 22 | jvalmat.col(j) = tst_cost + tst_proba*jvalues; 23 | } 24 | 25 | //to be modified because some actions are unavailable to some states. 26 | policy = index_max(mat(jvalmat(span::all,span(0,transitions.size()-2))),1); 27 | jval = arma::max(jvalmat,1); 28 | /*for (size_t k = 0; k < ns; k++) { 29 | jval(k) = jvalmat(k,policy(k)); 30 | }*/ 31 | 32 | if ( norm(jvalues-jval) < ϵ ) { 33 | converged = true; 34 | } 35 | ++i; 36 | jvalues.swap(jval); 37 | } 38 | 39 | jvalues.swap(jval); 40 | last_iter = i; 41 | } 42 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/markovdp.h: -------------------------------------------------------------------------------- 1 | #define ARMA_USE_HDF5 2 | //#define ARMA_USE_SUPERLU 1 3 | #define ARMA_64BIT_WORD 4 | #define DARMA_DONT_USE_WRAPPER 5 | 6 | #include 7 | #include 8 | //#include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "mkl_lapacke.h" 20 | //#include "mkl.h" 21 | 22 | ///////////////////////////////////////////////////////////////////// 23 | //gdb helpers 24 | template 25 | void print_matrix(Matrix matrix) { 26 | matrix.print(std::cout); 27 | } 28 | 29 | 30 | template void print_matrix(arma::mat matrix); 31 | template void print_matrix(arma::cx_mat matrix); 32 | template void print_matrix>(arma::Col); 33 | template void print_matrix(arma::uvec); 34 | template void print_matrix(arma::rowvec); 35 | template void print_matrix(arma::vec); 36 | ///////////////////////////////////////////////////////////////////// 37 | 38 | using namespace arma; 39 | 40 | typedef unsigned int uint; 41 | 42 | struct valarray_comp { 43 | bool operator()(const std::valarray& lhs, const std::valarray& rhs) const{ 44 | uint s1 = lhs.size(); 45 | uint s2 = rhs.size(); 46 | uint i=0, j=0; 47 | for ( ; i < s1 && j < s2; ++i, ++j ) { 48 | if ( lhs[i] < rhs[j] ) { 49 | return true; 50 | } 51 | if ( rhs[j] < lhs[i] ) { 52 | return false; 53 | } 54 | } 55 | return ( i==s1) && ( j tst): 71 | jvalues(jval), 72 | transitions(tst){} 73 | 74 | ~MarkovDP(){} 75 | 76 | void value_iteration(double, uint, vec&, uvec&, uint&); 77 | void set_jvalues(vec); 78 | void set_transitions(std::vector); 79 | 80 | private: 81 | vec jvalues; 82 | std::vector transitions; 83 | 84 | }; 85 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/occupancy_map.h5: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch14_markov_decision_processes/src/occupancy_map.h5 -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/to_UCN.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | cat $1 | perl -pe 'BEGIN { binmode STDIN, ":utf8"; } s/(.)/ord($1) < 128 ? $1 : sprintf("\\U%08x", ord($1))/ge;' > /tmp/$1 3 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/v0.0/main.cc: -------------------------------------------------------------------------------- 1 | #include "markovdp.h" 2 | 3 | void move(const std::valarray&, const std::valarray&, std::map, double, valarray_comp>&,double&); 4 | void move_rdm(const std::valarray&, const std::valarray&, std::map, double, valarray_comp>&,double&); 5 | void warp(const std::valarray&, std::map, double, valarray_comp>&,double&); 6 | 7 | int main(int argc, char** argv){ 8 | 9 | MarkovDP mdp; 10 | mdp.add_vertex(std::valarray({3,0}), 0.0); 11 | mdp.add_vertex(std::valarray({2,0}), 0.0); 12 | mdp.add_vertex(std::valarray({1,0}), 0.0); 13 | mdp.add_vertex(std::valarray({0,0}), 0.0); 14 | mdp.add_vertex(std::valarray({0,1}), 0.0); 15 | mdp.add_vertex(std::valarray({0,2}), 0.0); 16 | mdp.add_vertex(std::valarray({0,3}), 0.0); 17 | mdp.add_vertex(std::valarray({0,4}), 0.0); 18 | mdp.add_vertex(std::valarray({0,5}), 0.0); 19 | mdp.add_vertex(std::valarray({0,6}), 0.0); 20 | mdp.add_vertex(std::valarray({-1,6}), 0.0); 21 | mdp.add_vertex(std::valarray({-2,6}), 0.0); 22 | mdp.add_vertex(std::valarray({-3,6}), 0.0); 23 | mdp.add_vertex(std::valarray({-3,7}), 0.0); 24 | mdp.add_vertex(std::valarray({1,6}), 0.0); 25 | mdp.add_vertex(std::valarray({2,6}), 0.0); 26 | mdp.add_vertex(std::valarray({3,6}), 0.0); 27 | mdp.add_vertex(std::valarray({3,7}), 0.0); 28 | mdp.add_vertex(std::valarray({-3,8}), -100.0); 29 | mdp.add_vertex(std::valarray({3,8}), 100.0); 30 | 31 | mdp.add_move([](const auto& from, auto& to, double& cost){ 32 | return move_rdm(std::valarray({0,1}),from,to,cost);}); 33 | mdp.add_move([](const auto& from, auto& to, double& cost){ 34 | return move_rdm(std::valarray({1,0}),from,to,cost);}); 35 | mdp.add_move([](const auto& from, auto& to, double& cost){ 36 | return move_rdm(std::valarray({0,-1}),from,to,cost);}); 37 | mdp.add_move([](const auto& from, auto& to, double& cost){ 38 | return move_rdm(std::valarray({-1,0}),from,to,cost);}); 39 | mdp.add_move(warp); 40 | 41 | std::vector jvalues; 42 | double epsilon = 1e-3; 43 | uint max_iter = 1000; 44 | uint last_iter; 45 | mdp.value_iteration(epsilon, max_iter, jvalues, last_iter); 46 | 47 | std::cout << "last iter: " << last_iter << '\n'; 48 | for (size_t i = 0; i < jvalues.size(); i++) { 49 | std::cout << jvalues[i] << '\n'; 50 | } 51 | 52 | return 0; 53 | } 54 | 55 | void move(const std::valarray& dxy, const std::valarray& from, std::map, double, valarray_comp>& to,double& cost) { 56 | cost = -1.0; 57 | if (from[1] >= 7) { 58 | return; 59 | } 60 | to[from + dxy] = 1.0; 61 | } 62 | 63 | void move_rdm(const std::valarray& dxy, const std::valarray& from, std::map, double, valarray_comp>& to,double& cost) { 64 | cost = -1.0; 65 | if (from[1] >= 7) { 66 | return; 67 | } 68 | 69 | to[from + dxy] = 0.9; 70 | to[from - dxy] = 0.1/3; 71 | to[from + std::valarray({dxy[1],dxy[0]})] = 0.1/3; 72 | to[from - std::valarray({dxy[1],dxy[0]})] = 0.1/3; 73 | 74 | } 75 | 76 | 77 | void warp(const std::valarray& from, std::map, double, valarray_comp>& to,double& cost) { 78 | cost = 0.0; 79 | if (from[1] != 7) { 80 | return; 81 | } 82 | 83 | to[std::valarray({3,8})] = 0.5; 84 | to[std::valarray({-3,8})] = 0.5; 85 | 86 | } 87 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/v0.0/makefile: -------------------------------------------------------------------------------- 1 | help: 2 | @echo 3 | @echo "make lib <-- Compile lib." 4 | @echo "make build <-- Compile all." 5 | @echo "make clean <-- Clean up." 6 | @echo "make install <-- Compile and install" 7 | @echo "make dist <-- Create .tar.gz project file" 8 | @echo 9 | 10 | PROJECT_NAME = markovdp 11 | VERSION = 001 12 | 13 | ARCHIVE_DIR_NAME = $(PROJECT_NAME)-$(VERSION) 14 | ARCHIVE_NAME = $(ARCHIVE_DIR_NAME).tar.gz 15 | 16 | FILES_TO_BE_ARCHIVED = *.h *.cc Makefile 17 | 18 | INCLUDE_PATH = -I. -I/opt/intel/compilers_and_libraries/linux/mkl/include/ -I/usr/include/hdf5/serial 19 | #LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib64:. 20 | COMPILO = g++ 21 | CXXFLAGS = $(DEBUGFLAGS) -m64 -Wall -Wextra -Wshadow -Werror -pedantic $(INCLUDE_PATH) -Wno-deprecated-declarations -Wno-unused-variable -Wno-unused 22 | CXXFLAGS_SHARED = $(CXXFLAGS) -fPIC -shared 23 | LIBS = -L.# -L/usr/local/hdf5/lib/ -lfslam -larmadillo -lhdf5_cpp 24 | 25 | INSTALL_PREFIX = ./LocalInstall 26 | 27 | INSTALL_BIN_DIR = $(INSTALL_PREFIX)/bin 28 | INSTALL_LIB_DIR = $(INSTALL_PREFIX)/lib 29 | INSTALL_INCLUDE_DIR = $(INSTALL_PREFIX)/include/seif 30 | 31 | INSTALL_DIRS = \ 32 | $(INSTALL_BIN_DIR) \ 33 | $(INSTALL_LIB_DIR) \ 34 | $(INSTALL_INCLUDE_DIR) 35 | 36 | ########################################################## 37 | #patch for gcc: pre process to convert UTF-8 encoding to Unicode code point. 38 | #ref: https://gcc.gnu.org/wiki/FAQ#What_is_the_status_of_adding_the_UTF-8_support_for_identifier_names_in_GCC.3F 39 | PROCESS_UTF8 = ./to_UCN.sh 40 | 41 | #SHELL := /bin/sh 42 | #CC := g++ 43 | #CXX := g++ 44 | 45 | #memos 46 | #export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/intel/compilers_and_libraries/linux/mkl/lib/intel64_lin:/opt/intel/compilers_and_libraries/linux/lib/intel64:/usr/local/lib64:/usr/local/lib:. 47 | #export LIBRARY_PATH=/opt/intel/compilers_and_libraries/linux/mkl/lib/intel64_lin:/opt/intel/compilers_and_libraries/linux/lib/intel64 48 | #export MKL_VERBOSE=1 49 | #gnuplot 50 | #plot "poses.dat" using 91:92 pt 7 ps 0.1 lc rgb "black", "ldmarks.dat" using 1:2 pt 7 ps 0.6 lc rgb "green" 51 | 52 | COMPILER_OPTIONS := -m64 -Wall -Wextra -Wshadow -Werror -pedantic -Iinclude -Wno-unused-parameter -Wno-unused-variable -Wno-deprecated-declarations 53 | #INCLUDE_PATH = -I. 54 | 55 | #CXX_DEFINES = -DSHADERS_DIR=\"/Users/TACHER/sparklepp/src/shaders\" -DUSE_GLEW 56 | 57 | #CFLAGS := -std=c11 $(COMPILER_OPTIONS) 58 | #CXXFLAGS := -std=c++14 -Weffc++ $(COMPILER_OPTIONS) $(INCLUDE_PATH) 59 | 60 | DEBUGFLAGS := -g -O0 -D _DEBUG 61 | RELEASEFLAGS := -O2 -D NDEBUG 62 | ########################################################## 63 | 64 | # Fabrication des .o (lib) 65 | 66 | 67 | # Fabrication des .o (hors lib) 68 | 69 | markovdp.o : markovdp.cc 70 | $(PROCESS_UTF8) markovdp.h 71 | $(PROCESS_UTF8) markovdp.cc 72 | $(COMPILO) -c -std=c++17 $(CXXFLAGS_SHARED) /tmp/markovdp.cc 73 | 74 | main.o : main.cc 75 | $(COMPILO) -c -std=c++17 $(CXXFLAGS) main.cc 76 | 77 | # Fabrication de la lib 78 | 79 | libmarkovdp.so : markovdp.o 80 | $(COMPILO) -o libmarkovdp.so -shared markovdp.o 81 | 82 | # Fabrication de l'exécutable 83 | 84 | vi.bin : main.o libmarkovdp.so 85 | $(COMPILO) -o vi.bin main.o -lmarkovdp $(LIBS) 86 | 87 | # Les cibles de l'aide 88 | 89 | lib : 90 | 91 | build : vi.bin 92 | 93 | clean : 94 | @rm -f *~ *.o *.so *.bin 95 | @rm -rf $(ARCHIVE_DIR_NAME) $(ARCHIVE_NAME) 96 | @echo 97 | @echo "Cleaned up." 98 | 99 | install : fslam.bin libfslam.so 100 | @mkdir -p $(INSTALL_DIRS) 101 | @cp vi.bin $(INSTALL_BIN_DIR) 102 | @cp *.h $(INSTALL_INCLUDE_DIR) 103 | @echo "File installed in " $(INSTALL_BIN_DIR) $(INSTALL_LIB_DIR) $(INSTALL_INCLUDE_DIR) 104 | 105 | dist : 106 | @rm -rf $(ARCHIVE_DIR_NAME) $(ARCHIVE_NAME) 107 | @mkdir $(ARCHIVE_DIR_NAME) 108 | @cp $(FILES_TO_BE_ARCHIVED) $(ARCHIVE_DIR_NAME) 109 | @tar zcf $(ARCHIVE_NAME) $(ARCHIVE_DIR_NAME) 110 | @rm -rf $(ARCHIVE_DIR_NAME) 111 | @echo "File" $(ARCHIVE_NAME) " generated." 112 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/v0.0/markovdp.cc: -------------------------------------------------------------------------------- 1 | #include "markovdp.h" 2 | 3 | void MarkovDP::add_move(void (*fptr)(const std::valarray& from, std::map, double, valarray_comp>& to,double& cost)){ 4 | moves.push_back(fptr); 5 | } 6 | 7 | void MarkovDP::add_vertex(std::valarray coord,double jvalue){ 8 | vertices.insert(std::pair,double>(coord,jvalue)); 9 | } 10 | 11 | void MarkovDP::value_iteration(double ϵ, uint max_iter, std::vector& jvalues, uint& last_iter) const{ 12 | 13 | uint n = vertices.size(); 14 | uint k = moves.size(); 15 | double* costs = new double[k]; 16 | uint* nb_num = new uint[k*n]; 17 | 18 | uint l=0; 19 | jvalues.resize(n,0); 20 | std::vector proba; 21 | std::vector succ; 22 | 23 | for( auto const& vtx : vertices ) { 24 | auto const& coord = vtx.first; 25 | auto const& jvalue = vtx.second; 26 | 27 | int vertex_indx = (uint) std::distance(vertices.begin(),vertices.find(coord)); 28 | jvalues[vertex_indx] = jvalue; 29 | 30 | for (size_t j = 0; j < k; j++) { 31 | 32 | std::map, double, valarray_comp> m; 33 | 34 | moves[j](coord, m, costs[j]); 35 | double stay = 0.0; 36 | uint neighb = 0; 37 | 38 | for( auto const& tovtx : m ) { 39 | auto const& tocoord = tovtx.first; 40 | auto const& toprob = tovtx.second; 41 | 42 | auto it = vertices.find(tocoord); 43 | if (it != vertices.end()) { 44 | succ.push_back((uint)std::distance(vertices.begin(),it)); 45 | proba.push_back(toprob); 46 | neighb++; 47 | } 48 | else { 49 | stay += toprob; 50 | } 51 | } 52 | if (stay && (stay < 1)) { 53 | succ.push_back(vertex_indx); 54 | proba.push_back(stay); 55 | neighb++; 56 | } 57 | 58 | nb_num[l++] = neighb; 59 | 60 | } 61 | } 62 | 63 | bool converged = false; 64 | uint iter = 0; 65 | std::vector jvalues1(n); 66 | while ( !converged && (converged=true) && ( iter++ max) { 85 | max = curr; 86 | } 87 | } 88 | 89 | jvalues1[i] = (max>-60000.0)?max:jvalues[i]; 90 | if (std::abs(jvalues1[i]-jvalues[i]) > ϵ) { 91 | converged = false; 92 | } 93 | } 94 | 95 | std::swap(jvalues, jvalues1); 96 | } 97 | 98 | last_iter = iter; 99 | 100 | delete[] nb_num; 101 | delete[] costs; 102 | } 103 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/v0.0/markovdp.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | typedef unsigned int uint; 11 | 12 | struct valarray_comp { 13 | bool operator()(const std::valarray& lhs, const std::valarray& rhs) const{ 14 | uint s1 = lhs.size(); 15 | uint s2 = rhs.size(); 16 | uint i=0, j=0; 17 | for ( ; i < s1 && j < s2; ++i, ++j ) { 18 | if ( lhs[i] < rhs[j] ) { 19 | //std::cout << "true" << lhs[0] << ' ' << lhs[1] << ' ' << rhs[0] << ' ' << rhs[1] << '\n'; 20 | return true; 21 | } 22 | if ( rhs[j] < lhs[i] ) { 23 | //std::cout << "false" << lhs[0] << ' ' << lhs[1] << ' ' << rhs[0] << ' ' << rhs[1] << '\n'; 24 | return false; 25 | } 26 | } 27 | //std::cout << "?" << lhs[0] << ' ' << lhs[1] << ' ' << rhs[0] << ' ' << rhs[1] << '\n'; 28 | return ( i==s1) && ( j&, uint&) const; 51 | void add_move(void (*)(const std::valarray& from, std::map, double, valarray_comp>& to,double& cost)); 52 | void add_vertex(std::valarray,double); 53 | 54 | private: 55 | std::map, double, valarray_comp> vertices; 56 | std::vector&, std::map, double, valarray_comp>&,double&)> moves; 57 | 58 | }; 59 | -------------------------------------------------------------------------------- /ch14_markov_decision_processes/src/v0.0/to_UCN.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | cat $1 | perl -pe 'BEGIN { binmode STDIN, ":utf8"; } s/(.)/ord($1) < 128 ? $1 : sprintf("\\U%08x", ord($1))/ge;' > /tmp/$1 3 | -------------------------------------------------------------------------------- /ch15_partially_observable_markov_decision_processes/ch15_partially_observable_markov_decision_processes.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch15_partially_observable_markov_decision_processes/ch15_partially_observable_markov_decision_processes.pdf -------------------------------------------------------------------------------- /ch2_recursive_state_estimation/ch2_recursive_state_estimation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch2_recursive_state_estimation/ch2_recursive_state_estimation.pdf -------------------------------------------------------------------------------- /ch3_gaussian_filters/ch3_gaussian_filters.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch3_gaussian_filters/ch3_gaussian_filters.pdf -------------------------------------------------------------------------------- /ch3_gaussian_filters/data2.csv: -------------------------------------------------------------------------------- 1 | 0.52615,-0.90514 2 | 0.83947,-0.45785 3 | 0.93959,0.20901 4 | 0.11794,0.88933 5 | 0.78895,-0.35137 6 | 0.86243,-0.15525 7 | 0.45964,-1.2251 8 | 0.70383,0.94422 9 | 0.72824,0.37235 10 | 0.70225,0.61832 11 | 0.83315,0.44748 12 | 0.1464,-0.98011 13 | 0.72411,0.68487 14 | 1.0848,-0.055469 15 | 0.77049,-0.46728 16 | 0.88108,-0.43252 17 | 0.58482,0.64817 18 | 0.23397,1.0038 19 | 0.67254,0.63739 20 | 0.3637,1.1176 21 | 0.75751,-0.066862 22 | 0.47818,-0.95819 23 | 0.74502,-0.36291 24 | 0.62964,-0.79869 25 | 0.69866,-0.80524 26 | 1.0039,-0.27663 27 | 0.51997,-0.839 28 | 0.62456,-0.58405 29 | 1.044,-0.21112 30 | 1.077,0.22962 31 | 0.76621,-0.43725 32 | 1.0836,-0.28759 33 | 0.9435,-0.046912 34 | 0.88268,-0.3016 35 | 0.89896,0.46838 36 | 0.86601,-0.66355 37 | 0.99538,0.24331 38 | 0.51723,0.89129 39 | 0.43989,0.89442 40 | 0.90085,-0.36293 41 | 1.0067,0.31119 42 | 0.83158,-0.65331 43 | 0.78963,0.34662 44 | 0.32083,-0.99092 45 | 0.43809,-0.80053 46 | 0.75928,-0.54552 47 | 0.20828,-1.2384 48 | 0.91241,0.2801 49 | 1.0916,0.11433 50 | 0.64003,0.7368 51 | 1.0546,0.072843 52 | 0.97613,0.62042 53 | 0.8172,-0.64657 54 | 0.75262,0.42534 55 | 0.8869,-0.062061 56 | 0.98474,-0.5136 57 | 0.73425,0.87777 58 | 0.87634,0.28417 59 | 1.0093,-0.21296 60 | 1.0217,0.23303 61 | 1.0014,-0.18852 62 | 0.94568,-0.72619 63 | 1.0553,0.28436 64 | 0.99039,-0.3316 65 | 1.0601,-0.36046 66 | 1.1571,0.21865 67 | 0.96887,-0.24666 68 | 1.0723,-0.17409 69 | 0.27316,0.92456 70 | 0.079225,-0.91163 71 | 0.8001,-0.56992 72 | 0.58946,-0.62488 73 | 0.19856,1.0007 74 | 0.94287,-0.36459 75 | 0.18322,0.82559 76 | 0.79352,0.027338 77 | 0.86053,-0.069349 78 | 0.72758,-0.6736 79 | 0.67574,-0.80896 80 | 0.96552,-0.050098 81 | 0.79691,-0.43872 82 | 0.29647,0.91672 83 | 0.66682,-0.71198 84 | 0.47318,0.76236 85 | 0.98106,0.48071 86 | 0.81393,-0.38965 87 | 0.692,-0.64142 88 | 0.8733,-0.55449 89 | 1.0961,-0.48249 90 | 0.60777,-0.98914 91 | 0.50431,0.96524 92 | 0.91654,-0.52301 93 | 0.75586,-0.64875 94 | 0.94638,0.074084 95 | 1.0356,-0.065358 96 | 0.38905,-0.95167 97 | 0.9917,-0.38035 98 | 0.87202,0.22314 99 | 0.59767,0.67891 100 | 0.98996,-0.27018 101 | 0.71036,0.38814 102 | 0.54102,-1.009 103 | 0.90747,-0.23599 104 | 0.92942,0.20297 105 | -0.070969,-0.85123 106 | 1.1193,0.20012 107 | 0.92208,-0.39069 108 | 0.60138,-0.8054 109 | 0.77829,0.54075 110 | 0.90299,0.30611 111 | 0.65894,-0.454 112 | 0.86691,-0.2129 113 | 0.67823,-0.78626 114 | 0.94133,-0.72147 115 | 0.59046,0.93467 116 | 0.16113,1.0844 117 | 0.68137,0.48248 118 | 0.6876,-0.55946 119 | 1.0457,0.2354 120 | 0.90232,-0.30742 121 | 0.80029,-0.36306 122 | 0.71337,0.74228 123 | 0.11323,0.79775 124 | 0.96866,0.22311 125 | 0.6588,0.33346 126 | 0.90216,-0.63252 127 | 1.0099,-0.27299 128 | 1.054,-0.23287 129 | 0.54127,0.80054 130 | 1.1092,-0.32348 131 | 0.96672,0.12031 132 | 0.96374,0.42621 133 | 0.90839,-0.21121 134 | 0.96302,-0.61776 135 | 0.94018,-0.24682 136 | 0.96686,-0.60447 137 | 0.89139,-0.25502 138 | 0.90937,0.53803 139 | 1.106,-0.030352 140 | 0.8201,0.54796 141 | 0.53752,-0.87338 142 | 0.94142,-0.022783 143 | 0.6188,-0.85438 144 | 0.9567,0.082682 145 | 0.48671,-0.61022 146 | 0.94711,0.40753 147 | 0.8617,0.62153 148 | 0.87944,-0.20396 149 | 0.92881,0.19513 150 | 0.76106,0.46903 151 | 0.89063,0.43529 152 | 0.91762,0.074783 153 | 0.26493,-1.0354 154 | 1.1044,-0.33433 155 | 0.10642,-1.1284 156 | 1.0192,-0.28835 157 | 0.69468,0.60665 158 | 1.1421,0.36234 159 | -0.14366,1.0578 160 | 1.1072,-0.037907 161 | 0.95458,0.4581 162 | 0.99621,0.17292 163 | 0.8197,-0.51189 164 | 0.93755,0.0064064 165 | 0.93114,-0.38123 166 | 0.88045,-0.51585 167 | 0.85696,-0.42009 168 | 0.93298,-0.21399 169 | -0.31455,0.90495 170 | 1.0069,-0.42966 171 | 0.78422,-0.57828 172 | 0.7668,-0.36639 173 | 0.97091,-0.20944 174 | 0.6619,0.71613 175 | 0.75058,-0.71879 176 | -0.056412,-0.96581 177 | 0.89152,-0.081849 178 | 0.57143,0.495 179 | -0.29318,-1.0451 180 | 0.73529,0.41565 181 | 0.50485,0.84226 182 | 0.98367,0.083571 183 | 0.14279,-0.93535 184 | 1.1604,0.0078126 185 | 0.99525,0.33261 186 | 0.82931,0.48137 187 | 0.75307,0.34193 188 | 0.88425,0.41568 189 | 0.73436,0.65329 190 | 0.70641,-0.76057 191 | 0.70743,0.70939 192 | 0.22233,-0.92356 193 | 1.1561,0.12346 194 | 0.91845,0.23391 195 | 0.71892,0.80189 196 | 0.83946,-0.65611 197 | 0.99437,0.099263 198 | 0.5097,0.89151 199 | 1.1251,0.30704 200 | 0.58199,0.52305 201 | 1.0468,-0.067545 202 | 1.0633,-0.42397 203 | 0.74359,0.56554 204 | 0.89838,0.39601 205 | 0.65642,0.71535 206 | 1.1922,-0.15588 207 | 1.1615,-0.0055128 208 | 0.73545,0.63174 209 | 0.70421,-0.66127 210 | 0.83909,0.40431 211 | 1.0982,0.32967 212 | 0.29487,0.90099 213 | 0.09151,0.77905 214 | 0.55738,0.93049 215 | 0.59075,-0.65263 216 | 0.27987,-0.95759 217 | 0.65342,-0.87816 218 | 0.86457,0.42128 219 | 0.88204,-0.53379 220 | 0.70057,-0.71226 221 | 0.98046,-0.013315 222 | 0.90239,-0.23006 223 | 0.55004,-0.99535 224 | 0.91381,0.015235 225 | 0.79832,-0.83144 226 | 0.87944,-0.58571 227 | 0.76831,0.52082 228 | 0.96235,-0.32632 229 | 0.7953,-0.51723 230 | 0.42724,0.88486 231 | 0.68527,0.70645 232 | 0.9738,0.34316 233 | 0.98113,0.26711 234 | 0.59825,-0.62793 235 | 0.78814,-0.7103 236 | 0.80095,0.5524 237 | 0.97757,-0.050342 238 | 0.87634,-0.32018 239 | 0.17181,0.8617 240 | 1.0439,-0.30254 241 | 0.99116,-0.54521 242 | 0.97814,-0.40828 243 | -0.007882,-0.95342 244 | 1.0506,0.59611 245 | 0.78879,0.58537 246 | 1.1283,0.54958 247 | 0.63491,-0.70205 248 | 0.67213,-0.85989 249 | 0.77034,0.65468 250 | 0.83621,-0.41415 251 | 0.58704,0.68152 252 | 0.61914,-0.76244 253 | 0.8005,0.16531 254 | 0.74664,-0.64587 255 | 0.85871,-0.31591 256 | 1.0431,-0.43452 257 | 0.68566,0.74107 258 | 0.65053,0.64292 259 | 1.1075,0.097138 260 | 0.55997,0.85137 261 | 0.62143,-0.74252 262 | 0.40334,-0.95597 263 | 1.0753,0.33215 264 | 1.0282,0.11501 265 | 1.0392,0.052001 266 | 0.71352,-0.57445 267 | 0.86235,0.60556 268 | 0.92861,0.46986 269 | 0.5707,-0.86092 270 | 1.0728,-0.0077661 271 | 0.57908,0.90916 272 | 0.96563,-0.048697 273 | 0.0932,0.99359 274 | 1.0402,0.19271 275 | 1.1108,-0.084197 276 | 0.79431,-0.85293 277 | 1.2091,0.015059 278 | 0.81165,0.29755 279 | 0.16129,0.94727 280 | 1.0345,-0.4523 281 | 1.0265,-0.27691 282 | 0.94322,0.15692 283 | 1.1121,0.19651 284 | 0.79874,0.22972 285 | 0.45422,0.79222 286 | 0.78252,0.26881 287 | 1.0346,-0.39504 288 | 0.067505,0.87655 289 | 0.99932,-0.54756 290 | 1.105,0.29528 291 | 0.6494,0.81125 292 | 0.5205,0.85268 293 | 0.7053,-0.64198 294 | 1.0834,-0.10474 295 | 0.88694,-0.55538 296 | 0.88315,-0.12693 297 | 0.93815,-0.14917 298 | 0.77121,-0.80895 299 | 0.84905,0.17125 300 | 0.98856,0.16203 301 | 0.91441,-0.24971 302 | 1.0067,-0.2573 303 | 0.87311,-0.23634 304 | -0.12217,-0.91001 305 | 0.69718,-0.61981 306 | 0.71108,-0.67678 307 | 1.1252,0.0012967 308 | 1.0397,-0.028986 309 | 0.92219,-0.31345 310 | 0.88127,0.47047 311 | 0.7843,0.32731 312 | 0.82222,0.57525 313 | 1.218,-0.074484 314 | 1.0285,0.088767 315 | 0.80271,0.34293 316 | 1.0332,0.030721 317 | 0.8001,0.67832 318 | 0.77254,-0.54963 319 | 1.0004,0.28543 320 | 1.1372,-0.059647 321 | 0.97597,0.44496 322 | 0.80027,-0.50226 323 | 0.74345,0.72921 324 | -0.12398,-1.0607 325 | 0.78142,-0.45178 326 | 0.0072111,0.9825 327 | 0.34797,0.93573 328 | 0.99408,-0.058888 329 | 0.80264,-0.66482 330 | 0.87407,-0.43225 331 | 0.89232,-0.50224 332 | 0.75099,0.34989 333 | 0.49654,0.89229 334 | 0.89627,-0.094791 335 | 0.97123,0.092162 336 | 1.0107,-0.15012 337 | 0.72409,0.7723 338 | 0.90042,-0.36112 339 | 0.97789,0.2399 340 | 0.70001,-0.64326 341 | 0.75888,0.43259 342 | 0.63343,-0.73379 343 | 0.93117,0.28892 344 | 0.78424,-0.64243 345 | -0.69748,0.69122 346 | 0.93301,0.26335 347 | 0.87301,-0.63529 348 | 1.0594,-0.13055 349 | 0.59306,0.79642 350 | 1.1989,0.082941 351 | 0.34797,-0.81932 352 | 0.70821,0.40353 353 | 1.1536,-0.010927 354 | 0.69375,-0.56026 355 | 0.52791,0.7423 356 | 0.89118,0.039753 357 | 0.61167,-0.72544 358 | 0.31392,0.86669 359 | 0.6909,0.80358 360 | 0.55342,-0.69949 361 | 1.0737,0.18172 362 | 0.80947,0.62728 363 | 0.98616,-0.50058 364 | 0.8768,0.41593 365 | 0.8404,0.036481 366 | 1.0181,-0.2771 367 | 0.87503,-0.58567 368 | 0.55701,0.85121 369 | 1.0663,0.31153 370 | 0.97525,0.055467 371 | 0.92671,0.096186 372 | 0.41275,0.78725 373 | 1.0939,-0.10495 374 | 0.75364,-0.47956 375 | 0.92714,0.29036 376 | 0.72391,0.71227 377 | 0.88602,-0.31785 378 | 0.58727,-0.76386 379 | 0.36556,-0.85952 380 | 0.7199,-0.83428 381 | 0.38202,0.89483 382 | 0.79901,0.62715 383 | 0.72812,-0.6906 384 | 0.97711,-0.10663 385 | 0.95325,0.22081 386 | 0.77168,-0.53299 387 | 1.0039,0.32384 388 | 1.0388,-0.33624 389 | 0.78129,-0.6586 390 | 0.76681,-0.54336 391 | 0.96544,0.42425 392 | 0.97223,-0.34013 393 | 1.0207,-0.25006 394 | 1.012,-0.18156 395 | 0.77224,-0.47056 396 | 1.0873,-0.27886 397 | 0.9067,0.26403 398 | 0.82703,0.5149 399 | 1.0776,0.036124 400 | 1.1195,0.14567 401 | 0.83816,-0.52378 402 | 1.0055,-0.49392 403 | 0.96554,-0.27092 404 | 0.88391,0.37998 405 | 0.97448,-0.11074 406 | 0.74987,0.57707 407 | 0.67551,-0.55915 408 | 1.1356,0.10595 409 | 1.0858,-0.43505 410 | 0.92692,0.17002 411 | 0.96165,0.21838 412 | 0.099577,-0.95975 413 | 1.0295,-0.040331 414 | 1.0208,-0.58593 415 | 0.17122,1.0664 416 | 0.89797,0.76557 417 | 0.90967,-0.21956 418 | 0.92516,-0.38471 419 | 1.0206,0.13286 420 | 0.88893,0.26514 421 | 0.75633,0.78546 422 | 0.97145,-0.0030664 423 | 0.91478,0.44344 424 | 1.0325,-0.39861 425 | 0.86999,0.72089 426 | 1.0505,-0.41124 427 | 0.71807,-0.85089 428 | 0.50009,0.7081 429 | 0.77702,0.65614 430 | 0.85613,-0.13459 431 | 1.1533,-0.45097 432 | 0.56742,0.83775 433 | 1.0145,0.047274 434 | 1.03,0.03403 435 | 0.6714,0.75606 436 | 0.81682,-0.0033446 437 | 0.9896,-0.49346 438 | 1.0173,-0.18075 439 | 0.60376,0.85738 440 | 0.85597,-0.43619 441 | 0.8511,0.79796 442 | 1.0093,-0.27881 443 | 0.7924,-0.70588 444 | 0.77353,-0.72246 445 | 0.64775,0.44851 446 | 0.90157,0.32399 447 | 0.89507,0.29467 448 | 0.50631,-0.88021 449 | 0.67962,0.64864 450 | 0.97748,0.01183 451 | 1.0301,0.47243 452 | 0.54752,0.88333 453 | 0.70349,-0.81539 454 | 0.86766,0.25349 455 | 0.49156,-0.73722 456 | 0.18017,-0.97158 457 | 0.69447,0.89348 458 | 1.0179,-0.044079 459 | 0.98366,0.45377 460 | 0.68685,-0.6812 461 | 0.90007,0.17658 462 | 0.90583,-0.64661 463 | 0.98657,0.42105 464 | 0.68664,-0.60708 465 | 0.81508,0.23459 466 | -0.24524,0.9002 467 | 0.82906,0.45108 468 | 0.92946,-0.39646 469 | 1.0241,-0.17737 470 | 0.46694,-0.66161 471 | 1.0183,0.039576 472 | 0.7799,0.76 473 | 0.52207,0.63558 474 | 0.90218,-0.34711 475 | 0.96263,0.34661 476 | 1.017,0.28651 477 | 1.0297,0.076809 478 | 0.75382,-0.66093 479 | 0.88496,-0.50407 480 | 0.90005,0.063218 481 | 0.92106,-0.47271 482 | 0.96516,-0.33979 483 | 1.0256,0.48714 484 | 0.92158,0.48733 485 | 1.0259,0.096346 486 | 0.83366,0.18402 487 | 0.64625,0.89626 488 | 1.0332,-0.38549 489 | 0.7543,0.32691 490 | 0.98416,0.20261 491 | 0.73911,-0.31995 492 | 0.95571,-0.11675 493 | 1.0397,-0.049727 494 | 0.69007,-0.69171 495 | 0.58103,0.81776 496 | 1.0203,-0.031916 497 | 0.95332,-0.60005 498 | 1.0227,-0.351 499 | 0.46953,0.8799 500 | 0.33638,-0.91917 501 | 0.93071,0.28035 502 | 0.63529,0.69876 503 | 0.89815,0.45421 504 | 0.59851,-1.0233 505 | 0.34144,0.95654 506 | 0.98813,-0.1326 507 | 0.96724,0.013784 508 | 0.63704,-0.23623 509 | 1.1542,-0.063469 510 | 0.47288,-0.90716 511 | 0.89479,-0.22706 512 | 0.92621,-0.59209 513 | 0.92474,-0.097079 514 | 0.87131,0.37096 515 | 1.0632,0.086209 516 | 0.75664,0.62268 517 | 0.88495,-0.094508 518 | -0.18504,-1.0132 519 | 0.90934,-0.33897 520 | 0.62612,0.51085 521 | 0.6015,0.57058 522 | 0.13423,1.0009 523 | 1.0209,-0.62404 524 | 0.64664,0.73475 525 | 0.88998,0.15045 526 | 0.85744,0.49828 527 | 1.0611,0.23394 528 | 1.1095,0.54488 529 | 0.97419,0.22681 530 | 0.87024,-0.37401 531 | -0.081474,-0.98904 532 | 0.52696,-0.65224 533 | 0.54782,-0.70721 534 | 0.84878,-0.26376 535 | -0.23274,-1.0631 536 | 0.77057,0.31294 537 | 0.67808,0.69091 538 | 0.51938,-0.81717 539 | 1.1162,-0.090275 540 | 1.0466,-0.37456 541 | 0.78299,-0.31953 542 | 0.80823,0.47663 543 | 0.98647,-0.099224 544 | 0.91769,-0.39897 545 | 0.73356,-0.56916 546 | 1.0153,0.1487 547 | 0.92656,0.22833 548 | 0.97322,0.20824 549 | 0.89967,0.33399 550 | 0.80041,0.52899 551 | 0.8405,0.5735 552 | 0.79358,-0.27147 553 | 0.96199,0.049441 554 | 0.83002,0.28235 555 | 1.0638,0.040613 556 | 0.86183,0.34707 557 | 0.57994,-0.64982 558 | 0.77514,0.31349 559 | 0.99653,-0.36379 560 | 1.2055,-0.20713 561 | 0.13435,0.95421 562 | 1.0808,-0.18328 563 | 1.1382,0.24923 564 | -0.055557,-1.0187 565 | 0.97079,0.49887 566 | 0.36282,0.84809 567 | 0.78822,-0.60224 568 | 0.96309,-0.27693 569 | 0.71698,-0.45233 570 | 0.11587,-0.97156 571 | 0.60856,-0.93884 572 | 1.183,-0.027159 573 | 1.0897,-0.077657 574 | 0.90819,0.55304 575 | 1.0692,-0.18463 576 | 0.92093,-0.24508 577 | 0.32314,0.79198 578 | 0.80117,-0.54521 579 | 0.7225,-0.59078 580 | 1.035,0.3845 581 | 0.90942,0.50952 582 | 0.88071,0.051978 583 | 0.97919,-0.031711 584 | 0.80159,-0.6517 585 | 0.61672,0.68792 586 | 1.1317,0.18151 587 | 1.0108,0.40133 588 | 0.62721,0.71204 589 | 0.83919,0.62843 590 | 0.14728,-0.83194 591 | 0.73445,0.70839 592 | 0.87723,-0.19474 593 | 1.0998,0.1262 594 | 0.85084,0.19104 595 | 0.60718,-0.88015 596 | 0.4408,0.82982 597 | 1.0163,0.2366 598 | 0.80967,0.52622 599 | 0.89307,0.020047 600 | 1.0461,0.10355 601 | 1.0036,-0.10891 602 | 1.0048,-0.051897 603 | 0.7427,0.7485 604 | 1.0686,-0.024581 605 | 0.88988,-0.64592 606 | 0.98691,0.50938 607 | 0.92341,-0.61372 608 | 1.0309,-0.38002 609 | 1.0151,-0.56759 610 | 0.58956,0.72284 611 | 0.9686,0.10935 612 | 0.54491,-0.58078 613 | 1.0318,-0.30159 614 | 1.1034,0.4622 615 | 1.15,-0.030372 616 | 1.0336,-0.40779 617 | -0.03809,-0.96728 618 | 0.5898,-0.74328 619 | 1.0179,-0.02682 620 | 0.27453,-1.0075 621 | 1.0301,-0.39814 622 | 1.0662,-0.47201 623 | 0.85963,-0.51242 624 | 0.77927,-0.71066 625 | 0.93693,0.55698 626 | 0.41424,-0.77438 627 | 0.7032,0.46128 628 | 0.8968,-0.11165 629 | 0.90641,-0.142 630 | 0.7695,0.5666 631 | 0.922,0.34013 632 | -0.58173,0.79388 633 | 0.68882,-0.85066 634 | 0.91911,0.40746 635 | 0.83102,0.90264 636 | 0.80992,0.32235 637 | 1.0215,0.13313 638 | 1.0681,0.38987 639 | -0.45075,-0.91515 640 | 0.91487,0.38761 641 | 1.0924,0.34007 642 | 0.68398,0.61141 643 | 1.0211,-0.25668 644 | 0.90831,0.18728 645 | 0.95091,0.010622 646 | 0.43836,-0.69126 647 | 0.35952,1.0437 648 | 0.090165,-1.0474 649 | 0.91584,0.0050735 650 | 0.80016,-0.3805 651 | 0.91588,0.45869 652 | 1.1656,0.045027 653 | -0.11912,-1.0443 654 | 0.5351,0.86867 655 | 0.89536,-0.40075 656 | 0.57114,-0.91341 657 | 0.94967,-0.5698 658 | 0.92328,0.50967 659 | 0.79485,-0.075884 660 | -0.32174,-0.8262 661 | 0.61083,0.71626 662 | 0.9168,-0.24294 663 | 0.73261,0.67624 664 | 0.66198,0.701 665 | 0.93082,0.26763 666 | 0.90416,-0.10743 667 | 0.65009,0.96789 668 | 0.40652,-0.74321 669 | 0.85567,-0.37616 670 | 0.74627,-0.82708 671 | 0.98347,-0.45871 672 | 0.46136,-0.80908 673 | 0.55119,0.71846 674 | 1.0314,0.096491 675 | 1.0192,0.49532 676 | 0.98273,0.50867 677 | 0.54339,0.89621 678 | 0.79265,-0.41237 679 | 0.53722,-0.89798 680 | 0.5015,-0.78846 681 | 0.96649,0.35871 682 | 1.066,-0.0067221 683 | 0.98871,0.38529 684 | 0.81889,0.20776 685 | 0.77175,-0.28522 686 | 0.52897,-0.93861 687 | 0.86523,0.23613 688 | 0.51629,0.88891 689 | 0.65208,-0.76604 690 | 0.74746,0.73316 691 | 0.50099,0.97477 692 | 0.88528,-0.47328 693 | 0.9841,-0.0052361 694 | 0.87601,0.23741 695 | 0.68145,0.70379 696 | 0.71304,0.65455 697 | 0.98264,0.024916 698 | 1.0398,0.017542 699 | 0.96737,-0.45928 700 | 0.73959,0.13492 701 | 0.23622,-1.1079 702 | 0.9202,0.35067 703 | -0.010876,0.84698 704 | 0.76794,0.68785 705 | 0.54776,-0.96161 706 | 0.56874,0.80945 707 | 0.52826,-0.82604 708 | 0.54324,0.61329 709 | 0.8521,-0.035547 710 | 0.53598,-0.94468 711 | 0.80953,-0.74713 712 | 1.0871,0.12161 713 | 0.83006,0.53557 714 | 0.94187,-0.54526 715 | 0.71504,-0.61557 716 | 1.0475,-0.43973 717 | 1.002,0.44883 718 | 0.81585,0.62164 719 | 1.0767,0.10966 720 | 0.62935,0.67751 721 | 1.1464,-0.10199 722 | 0.89041,0.62695 723 | 0.91011,-0.61461 724 | 1.0896,0.21501 725 | 1.0838,0.32258 726 | 1.0683,-0.43792 727 | 0.95712,-0.16623 728 | 0.9546,0.42002 729 | 0.91099,0.62226 730 | 0.63242,-0.67607 731 | 0.80012,-0.39285 732 | 0.62845,0.76315 733 | 0.92976,0.47955 734 | 0.1811,0.97839 735 | 0.96784,-0.23563 736 | 0.90757,-0.064877 737 | 1.0379,-0.05378 738 | 1.087,-0.13703 739 | 0.71507,0.63037 740 | 0.72215,0.71136 741 | -0.97317,0.37771 742 | 0.55759,0.75234 743 | -0.065414,-0.94741 744 | 0.91825,-0.31111 745 | 1.088,0.18275 746 | 0.88315,-0.4469 747 | 0.91688,-0.2404 748 | 1.0642,-0.15639 749 | 1.0717,-0.38989 750 | 0.33123,-1.0814 751 | 1.0492,6.2186e-05 752 | 0.78995,-0.46041 753 | 0.92001,0.23445 754 | -0.18949,-0.81643 755 | 1.1077,0.011674 756 | 0.98682,-0.17281 757 | 1.0377,0.42799 758 | 0.92378,-0.50387 759 | 0.77519,-0.82831 760 | 0.93137,0.084359 761 | 0.86809,0.25842 762 | 0.56351,-0.56697 763 | 0.78814,-0.69282 764 | 0.9863,-0.19572 765 | 0.43101,-0.86864 766 | 0.33093,-0.88658 767 | 0.057285,-0.82837 768 | 0.98477,0.35327 769 | 1.186,0.14588 770 | 1.0922,-0.093137 771 | -0.66874,0.89383 772 | 0.90284,0.21312 773 | 0.8641,0.18527 774 | 1.0168,-0.096159 775 | 0.42926,-0.89686 776 | 0.51171,0.74028 777 | 0.634,0.74864 778 | 0.92106,0.45272 779 | 0.23953,-0.95704 780 | 0.98616,0.43802 781 | 1.0021,0.24617 782 | -0.06358,0.83215 783 | 1.023,-0.10408 784 | 0.9611,-0.57675 785 | 0.76854,-0.7303 786 | 0.8463,0.0075072 787 | 0.87589,-0.24759 788 | 0.80166,0.78924 789 | -0.036124,0.97064 790 | 0.68533,-0.83566 791 | 1.0221,-0.05429 792 | 1.0639,-0.10902 793 | 0.3981,-0.99561 794 | 1.0365,-0.055552 795 | 0.99236,-0.14037 796 | 0.94476,-0.21684 797 | 0.55619,0.63473 798 | 0.98598,0.57438 799 | 0.97545,-0.52133 800 | 0.85056,0.51378 801 | 0.94208,-0.36221 802 | 0.84888,-0.46973 803 | 0.84348,0.0055369 804 | 0.75621,-0.80123 805 | 1.02,-0.27111 806 | 1.03,-0.51097 807 | 0.9675,-0.21589 808 | 0.94809,-0.18107 809 | 0.34632,1.1102 810 | 1.026,0.44866 811 | 1.1795,-0.36278 812 | 0.69293,0.66014 813 | 0.70681,0.52919 814 | 0.79736,0.56667 815 | 1.0383,-0.14155 816 | 0.65042,-0.85265 817 | 0.85608,0.53896 818 | 0.94709,0.29677 819 | 0.73072,-0.76405 820 | 0.047319,1.0652 821 | 0.88792,-0.25638 822 | 0.83752,0.63057 823 | 0.71122,-0.6431 824 | 0.62863,0.60679 825 | 1.0412,-0.29623 826 | 0.96027,-0.20694 827 | 0.88647,0.54694 828 | 1.1918,-0.22604 829 | 0.33333,0.96077 830 | 0.53603,1.0204 831 | 1.1821,0.012987 832 | -0.055454,1.0139 833 | 0.71512,-0.53692 834 | 0.77336,-0.62632 835 | 0.48801,-0.84737 836 | 0.65929,-0.75028 837 | 0.85004,0.48542 838 | 0.91381,-0.22777 839 | 1.0066,0.42123 840 | 0.8539,0.046652 841 | 0.71695,-0.5109 842 | 0.87612,-0.16017 843 | 0.79652,-0.51711 844 | 1.0574,0.03912 845 | 0.99759,-0.034835 846 | 0.79849,0.46761 847 | -0.51672,-0.84521 848 | 0.15991,0.89163 849 | 0.73049,-0.71505 850 | 0.93698,0.48426 851 | 0.25264,-1.0413 852 | 1.0115,0.37587 853 | 0.62389,-0.55032 854 | 0.017765,0.79586 855 | 0.86507,0.12802 856 | 1.002,0.024139 857 | 0.90319,-0.47774 858 | 0.85891,-0.16349 859 | 0.85207,-0.12319 860 | 0.94074,0.090882 861 | 0.96659,-0.14778 862 | 0.72308,-0.91236 863 | 0.57612,-0.73946 864 | 1.0581,0.29158 865 | 0.92861,0.32887 866 | 0.7655,-0.52122 867 | 0.30572,0.90959 868 | 1.2135,0.091988 869 | 0.449,0.8906 870 | 1.0324,-0.34574 871 | 0.43247,1.0481 872 | 0.93319,0.13212 873 | 0.96548,0.41861 874 | 0.63194,-0.67761 875 | 0.98964,0.26246 876 | 0.287,1.1008 877 | 0.95226,0.016865 878 | 1.21,0.078982 879 | 0.79647,0.77845 880 | 0.95903,0.36573 881 | 1.0998,-0.086363 882 | 0.88435,0.13088 883 | 1.0136,0.63923 884 | 1.0985,-0.12068 885 | 0.67606,-0.71273 886 | 0.7242,0.72731 887 | 0.89564,0.19329 888 | 1.1218,0.0092416 889 | 0.69108,0.6186 890 | 0.39407,-0.98608 891 | 0.66205,0.72207 892 | 1.0195,0.0019366 893 | 0.88745,0.21586 894 | 0.73478,0.64531 895 | 0.4106,-0.83365 896 | 0.44914,1.0318 897 | 1.0825,-0.095046 898 | 0.020475,-1.062 899 | 0.45364,-1.083 900 | 0.86148,0.68237 901 | 1.0376,0.064884 902 | 1.0648,-0.067873 903 | 0.91452,0.24267 904 | 0.76928,-0.61458 905 | 0.86307,0.3371 906 | 0.99023,-0.31891 907 | 0.91601,0.043793 908 | 0.56158,0.98283 909 | 0.99985,0.32342 910 | 0.53967,0.78932 911 | 0.63676,0.66513 912 | 0.86004,0.13266 913 | 1.0778,-0.29828 914 | 0.9621,0.5393 915 | 0.66799,-0.81159 916 | 1.2062,-0.18065 917 | 0.98697,0.23694 918 | 0.9837,0.29664 919 | 0.91117,-0.54433 920 | 1.0639,0.065545 921 | 0.9257,0.11852 922 | 0.67415,-0.75324 923 | 0.69756,-0.71462 924 | 0.83021,-0.18836 925 | 0.54424,0.74166 926 | 1.0304,0.52403 927 | 0.38948,0.74626 928 | 1.0479,0.64582 929 | 1.0483,0.16615 930 | 0.78828,-0.060767 931 | 0.98039,0.07984 932 | 0.6909,0.74817 933 | 0.91318,0.23704 934 | 0.82865,0.74851 935 | 0.89594,-0.22804 936 | 0.10207,-1.0461 937 | 1.1275,-0.37023 938 | 1.033,0.21025 939 | 1.0154,-0.29644 940 | 1.006,-0.10197 941 | 0.41764,-0.79101 942 | 0.94014,-0.40661 943 | 0.84944,-0.50076 944 | 1.2,-0.071761 945 | 0.86557,0.39168 946 | 0.93075,0.41051 947 | 0.59223,0.59835 948 | 0.91901,0.80306 949 | 1.019,-0.4057 950 | 0.60842,0.87379 951 | 0.9725,0.029628 952 | 0.81061,-0.54434 953 | 0.91593,-0.50067 954 | 0.75075,-0.49692 955 | 0.67093,-0.64904 956 | 0.46816,0.6511 957 | 0.95347,-0.14871 958 | 0.74252,0.65721 959 | 0.8648,-0.4183 960 | 0.37398,0.86514 961 | 0.99877,0.21151 962 | 0.80916,-0.74329 963 | 0.90727,0.45407 964 | 1.0071,0.31747 965 | 0.36375,-0.98248 966 | 0.5775,-0.84776 967 | 1.0097,0.1176 968 | 0.99902,-0.57476 969 | 1.0255,-0.51518 970 | 0.77916,0.40872 971 | 0.7963,0.27006 972 | 0.65431,0.76368 973 | 1.0771,0.065072 974 | 0.51282,0.89284 975 | 0.94878,-0.29208 976 | 0.92534,0.006132 977 | 0.93648,-0.017134 978 | 1.0419,-0.045651 979 | 0.79239,-0.8357 980 | 0.93214,0.43349 981 | 0.91771,-0.66897 982 | 0.87178,-0.73756 983 | -0.2572,0.83628 984 | 0.74159,-0.84804 985 | -0.15078,-1.0879 986 | 0.99887,-0.20141 987 | 0.79846,-0.61424 988 | 0.18582,1.1767 989 | 0.99165,0.33369 990 | 1.008,0.10419 991 | 1.1655,-0.21228 992 | 1.0508,0.019858 993 | 0.33908,-1.0531 994 | 0.87618,-0.46098 995 | 0.6028,0.58376 996 | 0.54231,-0.78852 997 | 0.88648,-0.5555 998 | 1.0887,0.56328 999 | 1.0126,0.29 1000 | 0.94569,0.40719 1001 | -------------------------------------------------------------------------------- /ch3_gaussian_filters/data3.csv: -------------------------------------------------------------------------------- 1 | 0.8895,0.43887 2 | 0.80023,-0.44732 3 | 1.1549,0.41575 4 | 0.90642,0.30772 5 | 0.87559,0.61736 6 | 0.91276,0.38338 7 | 1.01,0.18776 8 | 0.77425,-0.29745 9 | 0.83219,0.61909 10 | 0.903,0.37277 11 | 0.74688,-0.30229 12 | 0.97652,0.089271 13 | 0.70089,-0.32178 14 | 0.82745,-0.75794 15 | 1.078,-0.19407 16 | 0.93788,0.18149 17 | 0.87286,0.38802 18 | 1.0257,-0.098788 19 | 0.93143,-0.016357 20 | 0.99346,0.25669 21 | 0.71572,-0.51977 22 | 1.0578,-0.26766 23 | 1.0838,-0.1044 24 | 0.93263,-0.27317 25 | 0.7744,-0.65673 26 | 1.0494,-0.5844 27 | 0.81297,-0.028061 28 | 1.0911,-0.355 29 | 0.83407,0.75617 30 | 1.0494,-0.20051 31 | 1.1309,0.10678 32 | 0.73147,-0.045256 33 | 0.74834,0.66801 34 | 0.75439,0.75008 35 | 0.9648,-0.12467 36 | 0.86323,0.46875 37 | 0.78629,-0.13525 38 | 0.80019,0.27222 39 | 0.5071,0.96291 40 | 0.99041,-0.15686 41 | 0.98343,-0.35715 42 | 0.90122,-0.45162 43 | 0.73043,0.54133 44 | 0.95821,0.34333 45 | 0.96283,0.32127 46 | 0.9106,-0.098523 47 | 1.0461,-0.37852 48 | 1.0448,-0.28609 49 | 0.91464,-0.34963 50 | 0.91275,-0.32902 51 | 1.0621,0.16509 52 | 0.92459,-0.094119 53 | 0.66351,0.40481 54 | 1.0126,-0.24179 55 | 0.92763,0.3805 56 | 0.75462,-0.73635 57 | 0.70912,0.79117 58 | 1.0489,0.14714 59 | 0.99483,0.057539 60 | 1.0366,-0.40284 61 | 0.97407,-0.29248 62 | 0.90961,-0.15856 63 | 0.81306,-0.23917 64 | 0.75589,0.67009 65 | 0.99912,0.16084 66 | 0.9015,0.098981 67 | 0.77422,-0.34145 68 | 0.79633,-0.66901 69 | 0.68342,0.77052 70 | 1.0696,-0.077077 71 | 0.78271,-0.13372 72 | 0.789,-0.58003 73 | 1.0748,0.36385 74 | 0.93303,-0.4295 75 | 0.69935,0.44301 76 | 0.46965,-0.83801 77 | 0.98687,0.26709 78 | 1.1011,0.11457 79 | 0.79416,0.34642 80 | 1.077,-0.15774 81 | 0.31334,-1.0634 82 | 0.93064,-0.4722 83 | 0.012035,1.1648 84 | 0.9357,0.44702 85 | 0.68249,-0.50683 86 | 1.0027,0.25893 87 | 0.97363,0.18589 88 | 0.33715,-0.85935 89 | 1.0734,-0.038822 90 | 0.48777,-0.85805 91 | 0.7949,0.64781 92 | 0.909,0.42324 93 | 0.87597,0.62578 94 | 0.47487,0.70848 95 | 0.92795,-0.0054978 96 | 0.82091,-0.11945 97 | 1.1488,0.17549 98 | 1.0656,0.044493 99 | 1.0384,-0.14369 100 | 1.1399,-0.13788 101 | 0.89528,-0.40131 102 | 0.93124,0.024192 103 | 0.74343,-0.60623 104 | 1.058,0.14822 105 | 1.1489,0.29327 106 | 1.0151,0.16464 107 | 0.87449,0.57109 108 | 0.96457,0.10289 109 | 0.947,0.1631 110 | 0.97095,-0.051226 111 | 0.82702,-0.11799 112 | 0.69188,0.63944 113 | 0.53385,-0.82335 114 | 0.74242,-0.54196 115 | 0.86905,-0.34896 116 | 0.96297,0.32885 117 | 0.67744,-0.55442 118 | 0.96989,-0.09776 119 | 1.056,0.15358 120 | 1.0362,-0.083913 121 | 0.98813,0.30024 122 | 0.93419,0.24284 123 | 0.4186,0.81464 124 | 0.69112,-0.75136 125 | 0.95176,-0.30261 126 | 0.65452,0.5273 127 | 1.029,-0.18493 128 | 0.79126,0.025075 129 | 0.86303,-0.19673 130 | 0.96544,0.67563 131 | 1.0935,0.38717 132 | 1.0624,0.38043 133 | 0.88594,0.20475 134 | 0.54975,-0.77755 135 | 0.47177,0.9024 136 | 0.92561,-0.28199 137 | 0.95148,0.098132 138 | 0.77672,0.83092 139 | 0.99833,0.023871 140 | 1.102,-0.22966 141 | 0.97455,0.32232 142 | 1.0125,-0.26266 143 | 0.88831,-0.44089 144 | 0.4885,-0.99242 145 | 1.084,-0.042725 146 | 1.1331,0.0018903 147 | 0.78458,0.62897 148 | 0.78455,0.35461 149 | 1.0527,0.36007 150 | 0.93864,-0.018636 151 | 0.83742,0.37714 152 | 0.61117,0.40674 153 | 1.2314,-0.23268 154 | 0.88224,0.11843 155 | 0.67658,-0.62419 156 | 0.87448,-0.35893 157 | 1.0146,0.12749 158 | 0.81286,-0.20925 159 | 1.0939,-0.14875 160 | 0.61405,0.57745 161 | 0.9374,-0.41016 162 | 1.009,0.070888 163 | 0.82782,-0.60803 164 | 0.75135,0.21597 165 | 0.99445,0.24256 166 | 0.63537,-0.47674 167 | 0.97414,-0.44457 168 | 0.93802,-0.34907 169 | 0.32563,-0.96864 170 | 0.87724,-0.56991 171 | 0.94949,-0.41291 172 | 0.98663,0.32029 173 | 0.96902,-0.26814 174 | 0.91257,-0.68157 175 | 0.90167,-0.48516 176 | 0.91743,0.37115 177 | 0.98616,0.078509 178 | 0.74764,-0.58877 179 | 0.026011,1.01 180 | 0.93679,0.2076 181 | 0.77977,0.65369 182 | 0.36824,0.91415 183 | 0.92347,0.60993 184 | 0.8671,0.016145 185 | 0.53245,-0.74858 186 | 0.75892,-0.8059 187 | 0.88671,-0.25574 188 | 0.98132,-0.11369 189 | 0.68092,0.77058 190 | 0.26638,0.85552 191 | 0.90625,0.45865 192 | 0.90184,-0.54109 193 | 0.98531,-0.000576 194 | 0.54438,1.0325 195 | 1.0464,0.1594 196 | 0.93216,-0.23174 197 | 1.0287,-0.46933 198 | 1.0044,0.042296 199 | 0.82494,-0.195 200 | 0.89793,0.1552 201 | 0.74113,0.47419 202 | 0.40327,0.71526 203 | 0.98883,-0.099924 204 | 0.35622,0.90583 205 | 0.66783,0.89459 206 | 0.95857,-0.33476 207 | 0.71734,0.75086 208 | 0.89364,0.2153 209 | 0.78578,-0.7544 210 | 0.83777,0.53066 211 | 0.82408,0.49218 212 | 0.9691,0.33231 213 | 0.94069,0.082879 214 | 0.83528,-0.61883 215 | 0.4652,0.92385 216 | 0.73682,0.53035 217 | 0.40586,0.87198 218 | 0.93126,0.38225 219 | 0.99724,-0.11383 220 | 0.9395,0.54739 221 | 0.73882,-0.57253 222 | 0.97251,0.13887 223 | 0.98779,-0.21339 224 | 1.0616,0.092515 225 | 1.0545,0.28845 226 | 0.80474,0.51283 227 | 1.0395,-0.18213 228 | 0.93098,-0.087324 229 | 0.31861,-1.021 230 | 0.87084,-0.6476 231 | 0.46002,0.79169 232 | 1.1375,0.043452 233 | 0.95753,0.20553 234 | 0.74745,-0.64165 235 | 1.0479,-0.12611 236 | 0.43145,-0.95405 237 | 0.65051,-0.65544 238 | 0.6833,-0.78796 239 | 0.49013,-0.71081 240 | 0.82882,-0.63455 241 | 0.90828,-0.59995 242 | 0.85627,-0.50722 243 | 0.89623,0.46115 244 | 0.43795,0.81922 245 | 0.97821,-0.25864 246 | 0.90361,0.15159 247 | 0.91036,-0.18373 248 | 0.9352,-0.46717 249 | 0.83051,-0.50116 250 | 0.4715,-0.72143 251 | 0.91028,0.36876 252 | 0.9002,0.0034539 253 | 0.67449,0.37159 254 | 0.89813,-0.4491 255 | 0.80522,-0.13283 256 | 0.94894,0.3162 257 | 0.76043,0.11019 258 | 0.67522,0.70968 259 | 0.87156,-0.40351 260 | 0.70196,0.7501 261 | 0.99438,-0.085815 262 | 0.87678,0.040368 263 | 0.98873,-0.06553 264 | 0.98103,-0.19146 265 | 0.59574,0.72995 266 | 1.2788,-0.52299 267 | 0.91953,0.53253 268 | 1.0901,-0.15222 269 | 1.0008,-0.09069 270 | 0.81094,0.21999 271 | 0.91347,-0.12876 272 | 0.72754,-0.54928 273 | 0.90304,-0.62749 274 | 0.89565,0.18564 275 | 1.0122,-0.14817 276 | 1.0765,0.035458 277 | 0.9113,0.43294 278 | 0.86997,0.040794 279 | 0.99152,-0.091801 280 | 1.0367,-0.1083 281 | 0.63497,0.51074 282 | 1.0639,0.39159 283 | 0.91635,0.58033 284 | 0.82967,0.57105 285 | 0.84198,0.56754 286 | 1.019,-0.31007 287 | 0.31949,1.116 288 | 0.9843,0.060486 289 | 0.93312,-0.010788 290 | 0.76031,-0.66907 291 | 0.75307,0.42797 292 | 0.97975,0.26685 293 | 1.3363,0.17506 294 | 0.94534,0.34506 295 | 0.84551,0.3179 296 | 1.0644,0.30355 297 | 0.22875,-0.853 298 | 0.93641,0.063746 299 | 0.97189,-0.051234 300 | 0.96344,0.31312 301 | 1.0107,-0.098508 302 | 1.0473,0.12732 303 | 1.009,0.042017 304 | 0.90561,-0.1241 305 | 0.91013,0.19463 306 | 0.97053,0.030218 307 | 0.87518,0.35998 308 | 1.0943,-0.36601 309 | 0.93861,0.67783 310 | 0.84973,-0.37212 311 | 0.99689,-0.28083 312 | 0.78041,-0.65761 313 | 1.082,-0.34985 314 | 0.82534,0.41818 315 | 0.95739,-0.010108 316 | 0.93236,-0.37052 317 | 0.8349,0.20643 318 | 0.96042,-0.28614 319 | 0.45133,-0.84239 320 | 0.79385,-0.32254 321 | 0.48336,0.92059 322 | 0.9647,0.18936 323 | 0.53779,-0.82057 324 | 0.87502,0.44777 325 | 0.6242,0.86877 326 | 1.0158,0.053158 327 | 0.87038,-0.19062 328 | 0.81875,0.40598 329 | 0.99828,-0.57831 330 | 1.0517,0.10526 331 | 0.20915,1.0185 332 | 0.80501,-0.47178 333 | 0.97376,0.084009 334 | 0.96561,0.26424 335 | 0.98148,0.1585 336 | 1.0553,0.50324 337 | 1.1376,-0.07708 338 | 0.858,-0.36097 339 | 1.0766,0.061306 340 | 0.65691,-0.84211 341 | 1.0813,-0.21075 342 | 0.50239,0.95914 343 | 1.0059,-0.2047 344 | 0.9487,0.10529 345 | 0.90175,0.40149 346 | 1.0073,0.090646 347 | 1.0628,-0.046894 348 | 1.014,-0.26479 349 | 0.83412,-0.034625 350 | 0.94699,-0.0055495 351 | 1.0427,0.048795 352 | 0.95136,0.42438 353 | 0.96009,0.053797 354 | 0.80421,-0.64468 355 | 0.8376,-0.44307 356 | 1.0442,0.38708 357 | 0.84024,0.59784 358 | 0.84057,-0.055558 359 | 0.79616,-0.5912 360 | 0.77236,-0.41306 361 | 0.8912,0.45044 362 | 0.87215,0.35271 363 | 1.0082,0.40409 364 | 0.74199,0.50281 365 | 0.98817,-0.095024 366 | 1.021,0.1979 367 | 0.85501,0.50086 368 | 0.8906,0.49391 369 | 0.32037,1.102 370 | 0.76664,-0.52523 371 | 1.0496,-0.076984 372 | 1.1108,0.093712 373 | 0.83422,-0.51616 374 | 0.54616,-0.7216 375 | 0.95908,0.12768 376 | 1.1032,-0.24431 377 | 0.92534,0.054084 378 | 0.98653,-0.40745 379 | 0.96317,-0.21947 380 | 0.91836,-0.00082186 381 | 0.42964,-0.92798 382 | 0.8928,0.53228 383 | 0.59482,-0.83807 384 | 0.95888,-0.027321 385 | 0.85749,0.17548 386 | 0.95116,0.16765 387 | 0.59262,0.70387 388 | 0.86206,-0.75662 389 | 1.0073,-0.22283 390 | 0.98077,-0.46415 391 | 0.808,0.93119 392 | 0.78752,0.60469 393 | 0.89796,0.25661 394 | 0.48954,-0.84963 395 | 1.1085,-0.26442 396 | 0.92305,0.2702 397 | 1.0277,0.1411 398 | 0.94069,-0.39698 399 | 0.76283,0.75338 400 | 0.9347,-0.36786 401 | 0.91254,0.22431 402 | 0.58991,0.82673 403 | 0.77506,0.83823 404 | 0.86912,-0.25597 405 | 1.096,0.47264 406 | 0.84122,0.6548 407 | 0.95873,-0.55226 408 | 0.97872,0.57435 409 | 0.48838,0.9885 410 | 0.94362,0.45115 411 | 0.8457,0.61267 412 | 1.1309,-0.11904 413 | 0.90278,-0.77155 414 | 0.92989,-0.24539 415 | 0.76084,0.78687 416 | 1.0718,0.033393 417 | 0.82518,0.35895 418 | 0.94736,-0.62017 419 | 0.60115,-0.8436 420 | 0.50568,-0.71721 421 | 1.0831,0.11829 422 | 0.51242,-0.82944 423 | 0.8746,0.67453 424 | 0.77292,0.4168 425 | 0.93518,-0.017755 426 | 0.83104,0.60503 427 | 0.97945,-0.031159 428 | 0.6239,-0.50958 429 | 0.92732,-0.077999 430 | 1.1727,-0.26746 431 | 0.8493,0.60587 432 | 1.1461,0.1084 433 | 0.91358,0.12363 434 | 0.88718,-0.28677 435 | 0.79784,-0.33113 436 | 1.0455,0.083899 437 | 1.0248,-0.31429 438 | 0.68426,0.62388 439 | 0.78706,-0.25035 440 | 0.78578,-0.39866 441 | 0.99601,-0.29577 442 | 0.9311,0.39539 443 | 1.1341,-0.27363 444 | 1.0678,-0.12503 445 | 0.81391,-0.73801 446 | 0.97986,0.24586 447 | 0.91061,-0.062164 448 | 0.77987,-0.24493 449 | 1.0352,-0.048195 450 | 0.86777,-0.08069 451 | 0.73648,0.22014 452 | 0.18738,0.98466 453 | 0.85814,0.52661 454 | 0.70412,0.75079 455 | 0.55753,0.85316 456 | 0.96825,-0.22023 457 | 0.98325,-0.31509 458 | 0.89535,-0.91986 459 | 0.94968,-0.62128 460 | 0.91857,-0.11547 461 | 0.96928,-0.054856 462 | 0.87481,0.23887 463 | 1.1058,0.025512 464 | 0.78779,-0.51626 465 | 1.12,-0.089493 466 | 0.61618,0.95645 467 | 0.97778,-0.15944 468 | 0.78645,-0.46391 469 | 0.91083,-0.11861 470 | 1.0966,0.17502 471 | 1.0835,-0.21045 472 | 0.96955,-0.025428 473 | 1.1021,-0.047968 474 | 0.85321,0.51059 475 | 0.77572,0.49269 476 | 0.83687,-0.24408 477 | 0.85262,-0.45986 478 | 0.60418,-0.87515 479 | 0.86372,0.50265 480 | 1.0169,-0.031051 481 | 0.92738,-0.10959 482 | 1.0273,0.079608 483 | 0.69036,-0.58884 484 | 0.92592,-0.54258 485 | 0.80833,0.0043123 486 | 0.9713,-0.12632 487 | 0.60171,-0.87962 488 | 0.93995,-0.42033 489 | 1.1303,-0.03856 490 | 0.83263,0.5341 491 | 1.0184,0.082539 492 | 0.95239,-0.066345 493 | 0.94397,-0.25933 494 | 1.0634,0.10283 495 | 1.0017,0.35056 496 | 0.74577,-0.51291 497 | 0.8538,0.44135 498 | 0.72129,-0.82361 499 | 0.95039,-0.20111 500 | 0.89167,-0.10971 501 | 0.69736,0.76849 502 | 1.0579,-0.23727 503 | 0.04373,0.90303 504 | 1.2385,-0.25515 505 | 1.0467,-0.37279 506 | 0.43557,1.0285 507 | 1.0569,-0.29212 508 | 1.0736,-0.35537 509 | 1.0582,0.17718 510 | 0.84799,-0.55538 511 | 0.56725,-0.67721 512 | 0.94965,-0.20111 513 | 0.85104,0.18759 514 | 0.82067,0.54729 515 | 0.7683,0.63921 516 | 0.9534,0.25942 517 | 0.9826,0.61545 518 | 0.36535,0.93235 519 | 1.1436,0.17207 520 | 0.84268,0.52402 521 | 0.8089,-0.088568 522 | 0.95232,0.52827 523 | 0.89066,-0.59501 524 | 1.0043,-0.31138 525 | 0.78019,-0.47327 526 | 0.96843,-0.33772 527 | 0.44864,0.80631 528 | 0.97554,0.061829 529 | 0.90209,-0.17788 530 | 0.81377,0.4582 531 | 1.1511,-0.12858 532 | 1.1854,0.19057 533 | 0.9077,0.39176 534 | 0.85322,-0.33687 535 | 1.0539,-0.078309 536 | 0.64546,0.84131 537 | 0.8396,0.10571 538 | 0.81881,0.54862 539 | 0.68505,0.60968 540 | 1.0041,0.13144 541 | 1.1265,0.080538 542 | 0.75973,0.50941 543 | 0.86514,-0.60166 544 | 0.74525,0.52499 545 | 0.44044,-0.94352 546 | 0.93066,-0.48289 547 | 0.99667,-0.25286 548 | 1.0547,-0.043195 549 | 0.76708,-0.72685 550 | 0.81753,0.23962 551 | 1.0835,0.078241 552 | 0.77468,-0.54098 553 | 0.92217,-0.48644 554 | 0.97359,-0.43977 555 | 1.0467,-0.29743 556 | 1.0418,-0.1381 557 | 1.0764,-0.19924 558 | 0.91309,-0.34336 559 | 0.85147,-0.1067 560 | 0.89586,-0.4431 561 | 1.065,0.20328 562 | 0.66315,0.79911 563 | 0.91907,0.50404 564 | 1.0861,-0.38706 565 | 1.1546,-0.099974 566 | 0.98903,0.075128 567 | 0.89358,-0.20616 568 | 0.91655,-0.1055 569 | 1.0347,0.49846 570 | 0.77739,-0.44381 571 | 1.0162,0.090449 572 | 0.91955,0.27985 573 | 0.85775,-0.23062 574 | 0.54611,-0.8054 575 | 0.77857,0.34116 576 | 1.1394,0.097125 577 | 0.78835,-0.068198 578 | 0.9929,0.61125 579 | 0.9559,0.037667 580 | 0.9546,-0.17225 581 | 0.93849,-0.34991 582 | 0.87571,-0.28279 583 | 0.92131,-0.22117 584 | 0.15578,1.0156 585 | 0.44853,-1.0196 586 | 1.0866,0.16114 587 | 1.0341,-0.064447 588 | 0.79458,-0.70526 589 | 1.1273,-0.1665 590 | 0.86298,0.20734 591 | 0.77094,0.57304 592 | 0.98001,0.049702 593 | 0.77253,0.16018 594 | 0.69281,0.69757 595 | 0.94023,0.12231 596 | 0.92433,-0.35087 597 | 0.69348,-0.70607 598 | 1.1082,0.54136 599 | 0.90633,-0.38705 600 | 0.75134,0.7286 601 | 0.94942,-0.0072589 602 | 1.014,-0.1403 603 | 0.96448,-0.18407 604 | 1.0402,-0.24313 605 | 0.68491,-0.56174 606 | 0.91988,0.33172 607 | 0.79601,-0.18866 608 | 0.97484,0.2315 609 | 0.75167,-0.86896 610 | 1.0258,0.22528 611 | 0.81182,-0.49934 612 | 0.75588,-0.54211 613 | 0.93995,-0.14789 614 | 0.93609,0.34366 615 | 0.91625,0.68499 616 | 0.64989,0.57786 617 | 0.89278,-0.15402 618 | 0.84064,-0.48429 619 | 0.067973,1.1523 620 | 0.9393,-0.24795 621 | 0.92176,-0.23134 622 | 0.92322,0.20111 623 | 1.0401,-0.67658 624 | 0.97616,-0.33267 625 | 0.62157,-0.57516 626 | 1.0368,-0.024826 627 | 0.63004,-0.55941 628 | 1.0719,0.1942 629 | 0.77961,0.0016431 630 | 0.99498,-0.28371 631 | 0.81355,-0.27312 632 | 0.93712,-0.2503 633 | 0.86508,-0.028436 634 | 0.96887,-0.315 635 | 0.92343,0.29402 636 | 0.86583,-0.40183 637 | 1.1295,0.33172 638 | 0.89545,-0.11023 639 | 0.5997,-0.98332 640 | 0.91872,0.22465 641 | 0.84909,0.60675 642 | 0.93159,0.62852 643 | 1.0412,0.15343 644 | 0.78049,0.70939 645 | 1.0556,0.20148 646 | 0.43162,0.88476 647 | 0.97443,0.20598 648 | 1.0107,0.11548 649 | 0.99866,0.32716 650 | 1.0527,0.18954 651 | 0.738,0.39557 652 | 0.76933,0.42345 653 | 0.75379,-0.50794 654 | 0.64151,-0.50871 655 | 0.84202,0.53134 656 | 0.84022,0.50926 657 | 0.80545,-0.43213 658 | 0.97971,-0.0032457 659 | 0.42508,0.7589 660 | 0.71388,-0.42675 661 | 1.0176,0.31678 662 | 0.59493,-0.6343 663 | 1.1466,-0.10613 664 | 0.85659,0.16016 665 | 0.99032,0.046208 666 | 0.81752,0.47173 667 | 0.98067,-0.10462 668 | 0.86872,0.55133 669 | 0.97449,0.22324 670 | 1.112,-0.15997 671 | 0.94883,0.084015 672 | 0.97646,0.066576 673 | 0.90878,0.46053 674 | 0.97686,0.4466 675 | 1.0115,0.22979 676 | 0.83601,0.073222 677 | 0.82917,-0.50003 678 | 0.68673,-0.64972 679 | 0.75716,0.59571 680 | 0.60954,0.81112 681 | 0.94379,0.20584 682 | 1.1594,-0.010712 683 | 0.94403,0.42246 684 | 0.69719,-0.75823 685 | 1.2011,-0.22866 686 | 0.49903,-0.78089 687 | 1.0139,0.31487 688 | 1.0611,0.084202 689 | 0.92209,0.20171 690 | 0.99573,-0.28299 691 | 0.45163,-0.97039 692 | 0.98138,0.42736 693 | 1.1365,-0.19783 694 | 1.0267,0.15204 695 | 0.6693,0.41629 696 | 1.147,0.18685 697 | 0.78722,-0.50782 698 | 0.8646,-0.2203 699 | 0.90799,-0.13347 700 | 0.8518,0.43834 701 | 0.65975,0.52078 702 | 1.1663,-0.61833 703 | 0.77969,-0.49488 704 | 1.0863,0.086841 705 | 1.0685,-0.01851 706 | 0.91023,-0.083486 707 | 0.77268,-0.66701 708 | 0.95039,-0.58182 709 | 1.0717,-0.076197 710 | 0.70328,0.46962 711 | 0.92191,0.55234 712 | 0.98695,0.050403 713 | 1.0453,-0.12831 714 | 0.94327,-0.53553 715 | 0.9999,0.42245 716 | 1.0067,0.62622 717 | 1.0263,-0.17587 718 | 1.1494,0.27679 719 | 0.9065,0.189 720 | 1.1017,0.50998 721 | 0.74706,0.72532 722 | 0.78049,-0.59248 723 | 0.966,-0.044313 724 | 0.9218,-0.19304 725 | 0.85548,-0.56617 726 | 1.0371,-0.087137 727 | 0.96687,0.3795 728 | 0.94915,-0.3523 729 | 0.56159,-0.73929 730 | 0.92194,-0.4672 731 | 0.91264,0.31382 732 | 0.79105,-0.46373 733 | 0.93485,0.41182 734 | 0.97829,-0.30271 735 | 1.0428,0.16317 736 | 0.80716,-0.25355 737 | 0.88549,0.40565 738 | 0.70017,-0.6727 739 | 0.81389,0.76609 740 | 0.87421,-0.039535 741 | 1.0107,0.064884 742 | 0.77674,-0.57469 743 | 1.0286,-0.1641 744 | 0.91832,-0.12294 745 | 0.7409,-0.76141 746 | 0.90069,0.48341 747 | 0.53896,0.81303 748 | 0.87544,0.55417 749 | 1.2063,-0.20913 750 | 0.80125,0.1989 751 | 1.0933,0.20202 752 | 1.0821,0.10177 753 | 0.49975,-0.6854 754 | 0.80356,-0.58792 755 | 0.95518,-0.2662 756 | 0.91618,0.35583 757 | 1.0411,0.029935 758 | 1.0231,-0.29839 759 | 0.53976,-0.83369 760 | 0.74701,-0.77564 761 | 0.84128,0.57197 762 | 0.98786,0.020415 763 | 1.1792,0.13264 764 | 0.91523,-0.15314 765 | 0.18618,0.99927 766 | 1.1671,-0.31846 767 | 0.93934,-0.27643 768 | 0.971,0.20486 769 | 0.86439,0.62285 770 | 1.0069,-0.1033 771 | 0.98005,0.39605 772 | 0.90754,0.49027 773 | 1.1281,-0.12152 774 | 0.76888,-0.82224 775 | 0.74697,0.36909 776 | 1.0776,-0.12555 777 | 0.67605,0.64049 778 | 0.91744,-0.45866 779 | 1.0631,-0.03613 780 | 0.77432,-0.22185 781 | 0.56785,-0.79007 782 | 0.92344,-0.42202 783 | 0.32778,-0.84738 784 | 0.54947,0.8589 785 | 0.911,0.20282 786 | 0.9064,0.31845 787 | 1.0612,0.29698 788 | 0.71356,0.52269 789 | 0.7123,0.73824 790 | 1.0427,-0.19013 791 | 0.93256,-0.3327 792 | 0.85656,0.39663 793 | 0.48532,0.81304 794 | 1.1515,-0.14097 795 | 0.94103,-0.38467 796 | 1.1486,0.14783 797 | 1.0706,-0.50399 798 | 0.8768,-0.46549 799 | 0.90376,0.18002 800 | 1.0547,-0.27197 801 | 0.89011,0.33928 802 | 0.8029,-0.45855 803 | 0.98435,0.3202 804 | 1.0288,0.17798 805 | 0.98617,0.3031 806 | 0.73552,0.44791 807 | 0.86655,0.44778 808 | 0.81953,-0.69687 809 | 0.62825,0.87454 810 | 0.793,-0.6437 811 | 0.57665,-0.58722 812 | 0.85087,0.56843 813 | 0.92737,-0.36274 814 | 0.61235,-0.8104 815 | 0.92956,-0.11631 816 | 0.50485,0.85746 817 | 0.90532,0.48853 818 | 0.95653,0.42573 819 | 0.86355,-0.51185 820 | 0.85794,-0.3023 821 | 0.72903,0.38518 822 | 0.76998,0.46056 823 | 0.93486,0.42764 824 | 1.1424,0.42318 825 | 0.92383,-0.42462 826 | 1.0293,-0.21742 827 | 0.86171,0.55419 828 | 0.77047,0.68409 829 | 0.89838,0.17399 830 | 0.93742,0.21436 831 | 0.86982,0.16981 832 | 0.95812,-0.26401 833 | 0.92609,0.36378 834 | 1.0771,0.073634 835 | 0.93024,0.22711 836 | 0.90173,-0.27372 837 | 1.0191,0.25095 838 | 0.81025,-0.31692 839 | 0.58346,0.89038 840 | 0.83046,-0.020633 841 | 0.9419,0.23568 842 | 0.59661,-0.76964 843 | 0.7747,0.37597 844 | 0.96081,-0.28106 845 | 1.0151,0.27542 846 | 1.0017,-0.35307 847 | 0.91232,0.22677 848 | 0.97233,-0.24366 849 | 0.96095,-0.33823 850 | 0.75945,-0.61907 851 | 0.78398,-0.53815 852 | 1.0539,0.32027 853 | 0.87265,0.64446 854 | 1.0316,0.13684 855 | 0.98714,-0.32647 856 | 0.87609,-0.40767 857 | 0.83894,-0.29973 858 | 0.66475,0.72914 859 | 1.0481,0.25723 860 | 0.99658,-0.67873 861 | 0.67963,-0.68835 862 | 0.77931,-0.41964 863 | 1.06,-0.15721 864 | 0.63535,0.90394 865 | 1.0313,0.097106 866 | 0.69363,-0.65275 867 | 0.87852,-0.25755 868 | 0.98337,-0.40146 869 | 0.90031,0.484 870 | 0.59605,0.90948 871 | 0.56239,0.81234 872 | 1.0312,-0.26879 873 | 0.6231,0.52072 874 | 1.0225,0.082309 875 | 0.98614,0.15902 876 | 0.93374,-0.54854 877 | 0.9801,-0.15151 878 | 0.82267,-0.32398 879 | 0.96369,-0.13723 880 | 0.93867,-0.39541 881 | 0.83699,-0.28765 882 | 0.73625,-0.87466 883 | 0.87535,0.45496 884 | 0.91599,-0.55266 885 | 1.0336,0.20405 886 | 0.9031,-0.26905 887 | 0.80197,-0.4865 888 | 0.43437,-0.7446 889 | 1.0784,-0.028822 890 | 1.027,0.28258 891 | 0.69163,0.78315 892 | 0.9038,0.28678 893 | 0.85437,-0.56079 894 | 0.99989,-0.10568 895 | 0.87729,0.5665 896 | 1.14,0.14258 897 | 0.93271,-0.25296 898 | 1.0048,0.1373 899 | 0.78104,0.4033 900 | 0.90996,-0.13909 901 | 1.0346,-0.41586 902 | 0.8661,-0.43685 903 | 0.78154,0.30367 904 | 1.0251,0.20415 905 | 1.0288,-0.39341 906 | 0.78038,-0.84537 907 | 1.0811,-0.021881 908 | 0.92441,0.054937 909 | 1.0349,0.27212 910 | 0.73135,0.64719 911 | 1.0321,-0.0030217 912 | 0.82766,-0.40249 913 | 0.50375,0.94787 914 | 0.92361,-0.071684 915 | 1.1643,-0.070192 916 | 0.95006,0.23352 917 | 0.81759,-0.75118 918 | 1.0435,0.020792 919 | 0.94963,-0.13303 920 | 0.63206,0.99093 921 | 0.85126,0.54341 922 | 1.0833,0.17873 923 | 0.78814,-0.60463 924 | 0.88542,0.32334 925 | 1.0384,-0.26965 926 | 1.0341,-0.18747 927 | 0.6597,-0.90123 928 | 0.51089,-0.88296 929 | 0.99227,0.12426 930 | 0.96393,0.29716 931 | 0.87733,-0.26089 932 | 0.95928,0.23981 933 | 1.044,0.057376 934 | 0.89455,-0.65199 935 | 0.74444,-0.62626 936 | 0.87991,0.49977 937 | 1.0482,0.32043 938 | 1.0971,-0.029046 939 | 0.85311,-0.53454 940 | 0.88357,0.32027 941 | 0.95062,0.20723 942 | 0.52191,-0.62626 943 | 0.88913,0.35356 944 | 0.83547,-0.01988 945 | 0.92009,0.26004 946 | 0.73059,-0.54671 947 | 0.75645,0.86842 948 | 1.0118,0.44005 949 | 0.96833,-0.36503 950 | 0.92906,-0.13379 951 | 0.89467,-0.3581 952 | 0.80709,0.27112 953 | 1.052,0.41145 954 | 0.79864,0.17962 955 | 1.0551,0.28452 956 | 0.84997,0.18952 957 | 0.86287,-0.61512 958 | 0.914,0.41509 959 | 0.98526,-0.39371 960 | 1.0929,0.12697 961 | 0.82847,-0.59484 962 | 0.98553,0.32254 963 | 0.83494,0.27703 964 | 0.78782,0.4326 965 | 1.0086,-0.31585 966 | 0.97846,0.43082 967 | 0.94524,-0.43421 968 | 0.81818,-0.23301 969 | 1.0681,-0.13001 970 | 1.0896,0.35997 971 | 0.81745,0.65201 972 | 0.97608,-0.12228 973 | 0.89054,0.0090721 974 | 0.96688,0.14894 975 | 1.0476,-0.38929 976 | 0.75557,0.75575 977 | 0.86314,0.5347 978 | 0.94744,0.34397 979 | 0.89656,-0.33912 980 | 0.66344,-0.785 981 | 0.78344,-0.32858 982 | 0.8184,0.27144 983 | 0.914,-0.30796 984 | 1.0755,0.13694 985 | 0.90207,-0.31171 986 | 1.0839,0.23575 987 | 0.91158,-0.017843 988 | 0.96987,0.14198 989 | 0.59823,-0.75858 990 | 0.83602,0.66556 991 | 1.0519,-0.13691 992 | 0.92741,0.30356 993 | 0.51246,0.83655 994 | 0.71086,0.14893 995 | 0.69265,0.51843 996 | 1.0733,-0.20356 997 | 1.0727,0.69073 998 | 1.0142,0.073047 999 | 1.0038,-0.070691 1000 | 1.0434,-0.058104 1001 | -------------------------------------------------------------------------------- /ch3_gaussian_filters/simmov.m: -------------------------------------------------------------------------------- 1 | %simulation of random variable (x+cos?, y+sin?) 2 | function r = simmov(s1, s2, n) 3 | theta = normrnd(0, sqrt(s2), [n,1]) 4 | xy = mvnrnd([0 0], s1*eye(2), n) 5 | r = xy + [cos(theta) sin(theta)] 6 | end 7 | -------------------------------------------------------------------------------- /ch4_nonparametric_filters/ch4_nonparametric_filters.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch4_nonparametric_filters/ch4_nonparametric_filters.pdf -------------------------------------------------------------------------------- /ch4_nonparametric_filters/detect_circle.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %returns arclength of unit circle intersecting cell 4 | 5 | % ============================================================== 6 | 7 | function b = detect_circle( x, y, c ) 8 | 9 | if (x+c/2 < sqrt(2)/2 && y+c/2 < sqrt(2)/2 )||(x-c/2 > 1 || y-c/2 > 1 ) 10 | b = 0; 11 | else if (x^2+y^2+1/2*c^2+c*(x+y) >1 && x^2+y^2+1/2*c^2-c*(x+y) <1) 12 | [x1,y1] =linecirc(inf, x-c/2, 0, 0, 1); 13 | [x2,y2] =linecirc(inf, x+c/2, 0, 0, 1); 14 | [x3,y3] =linecirc(0, y-c/2, 0, 0, 1); 15 | [x4,y4] =linecirc(0, y+c/2, 0, 0, 1); 16 | %strangely linecirc may return NaN in columns. 17 | A = [x1(1,:) x2(1,:) x3(1,:) x4(1,:); y1(1,:) y2(1,:) y3(1,:) y4(1,:)]; 18 | B = A(1,:); 19 | C = A(2,:); 20 | ind = find(abs(B-x)<=c/2+1e-15 & ~isnan(B) ); 21 | B = B(ind);C = C(ind); 22 | ind = find(abs(C-y)<=c/2+1e-15 & ~isnan(C) ); 23 | B = B(ind);C = C(ind); 24 | b = acos([B(1) C(1)]*[B(2) C(2)]'); 25 | else 26 | b = 0; 27 | end 28 | end 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /ch4_nonparametric_filters/epankde.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %bivariate Epanechnikov kernel density estimate 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %X: data whose underlying density is to be estimated. 7 | %xi: defines grid where the density is to be evaluated. 8 | %h: window width 9 | 10 | %f: values of density estimate. 11 | 12 | % ============================================================== 13 | 14 | function [ f ] = epankde(X,xi,h) 15 | m = size(xi, 2); 16 | n = size(X, 1); 17 | f = zeros(m,m); 18 | 19 | [x1,x2] = meshgrid(xi); 20 | 21 | [B,I] = sort(X(:,1)); 22 | X = [B X(I,2)]; 23 | 24 | for ii=1:m 25 | for jj=1:m 26 | Y = bsxfun(@minus, [x1(ii,jj) x2(ii,jj)],X); 27 | first = find(Y(:,1)>=-h,1,'first'); 28 | last = find (Y(:,1)<=h, 1, 'last'); 29 | if isempty(first) || isempty(last) 30 | f(ii,jj)=0; 31 | else 32 | Y = Y(first(1):last(1), :); 33 | N = (1-1/h^2.*dot(Y,Y,2)); 34 | N = N(N >=0); 35 | f(ii,jj) = 2/(pi*h^2*n)*sum(N,1); 36 | end 37 | end 38 | end 39 | 40 | -------------------------------------------------------------------------------- /ch4_nonparametric_filters/histogram_filter_1.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | %Implementation of histogram filter algorithm. 3 | %written by Pierre-Paul TACHER (ptacher@gmail.com). 4 | % ============================================================== 5 | 6 | %motion model matrix. 7 | A = [1 1;0 1]; 8 | 9 | %initial , mean, covariance matrix 10 | S = [2.5 2;2 2]; 11 | mu=[0 0]; 12 | 13 | %noise covariance. tweaked to avoid singularity. 14 | R = [1/4 1/2;1/2 1.001]; 15 | R1 = inv(R); 16 | d = det(R); 17 | 18 | cell = 0.2; 19 | [xmin ymin] = deal(-20, -20); 20 | 21 | %size of square grid. 22 | n = 200; 23 | 24 | prob1 = zeros(n, n); 25 | prob2 = zeros(n, n); 26 | 27 | %precompute transition probabilities for gaussian noise. 28 | %requires (2n-1)^2 probabilities. needs to be done only once. 29 | [X1,Y1] = meshgrid(-(n-1)*cell:cell:(n-1)*cell); 30 | X1 = reshape(X1,(2*n-1)^2,1); 31 | Y1 = reshape(Y1,(2*n-1)^2,1); 32 | Z1= [X1 Y1]; 33 | 34 | [P,D]=eig(R1); 35 | U = sqrt(D)*P'; 36 | DD=sum((Z1*U').^2, 2); 37 | D = zeros(2*n-1, 2*n-1); 38 | D = exp(-1/2*(reshape(DD,2*n-1,2*n-1))); 39 | 40 | %generate coordinates of cell centers. 41 | [X,Y]=meshgrid(xmin:cell:xmin+(n-1)*cell); 42 | 43 | X = reshape(X,n^2,1); 44 | Y = reshape(Y,n^2,1); 45 | Z= [X Y]; 46 | 47 | %compute initial distribution. 48 | D1 = pdist2(Z, mu, 'mahalanobis', S); 49 | 50 | prob1 = exp(-1/2*reshape(D1.^2,n,n)); 51 | eta1 = 1./sum(sum(prob1,1),2); 52 | prob1 = eta1*prob1; 53 | 54 | figure; 55 | %plotting takes a while. 56 | plotdistrib3(xmin:cell:xmin+(n-1)*cell,xmin:cell:xmin+(n-1)*cell,prob1, cell); 57 | 58 | for ee=1:3 59 | 60 | %motion step 61 | 62 | indexmap = @(l) max(min(l-floor(l/n)+1-ymin/cell,n^2), 1); 63 | 64 | %move probability masses according to X -> AX linear map. 65 | prob1 = prob1(indexmap(reshape(1:n^2, n, n,1)')); 66 | 67 | %this should be vectorized probably.or using blockproc-like function. 68 | for ii=1:n 69 | for jj=1:n 70 | prob2(ii,jj) = sum(sum((D(n+1-jj:2*n-jj , n+1-ii:2*n-ii )).*prob1,1),2); 71 | end 72 | end 73 | 74 | eta = 1./sum(sum(prob2,1),2); 75 | prob2 = eta.* prob2; 76 | 77 | figure; 78 | plotdistrib3(xmin:cell:xmin+(n-1)*cell,xmin:cell:xmin+(n-1)*cell,prob2, cell); 79 | prob1 = prob2; 80 | end 81 | 82 | %measurement update. 83 | z = 5; 84 | Q = 10; 85 | N = exp(-1/(2*Q)*((xmin:cell:xmin+(n-1)*cell)-z).^2); 86 | 87 | prob1 = bsxfun(@times, N', prob1); 88 | eta = 1./sum(sum(prob1,1),2); 89 | prob1 = eta.* prob1; 90 | 91 | figure; 92 | plotdistrib3(xmin:cell:xmin+(n-1)*cell,xmin:cell:xmin+(n-1)*cell,prob1, cell); 93 | -------------------------------------------------------------------------------- /ch4_nonparametric_filters/histogram_filter_2.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | %Implementation of histogram filter algorithm. 3 | %written by Pierre-Paul TACHER (ptacher@gmail.com). 4 | % ============================================================== 5 | 6 | %initial , mean, covariance matrix 7 | S = [0.01 0;0 0.01]; 8 | mu=[0 0]; 9 | 10 | cell = 0.015; 11 | [xmin ymin] = deal(-1.5, -1.5); 12 | 13 | %size of square grid. 14 | n = 200; 15 | 16 | prob1 = zeros(n, n); 17 | prob2 = zeros(n, n); 18 | 19 | %precompute transition probabilities. 20 | %requires (2n-1)^2 probabilities. needs to be done only once. 21 | [X1,Y1] = meshgrid(0:cell:(n-1)*cell); 22 | 23 | tmp = arrayfun(@(x, y) detect_circle(x, y, cell), X1, Y1); 24 | D = zeros(2*n-1, 2*n-1); 25 | tmp = [tmp(n:-1:2,:);tmp]; 26 | D = [tmp(:,n:-1:2) tmp]; 27 | 28 | %generate coordinates of cell centers. 29 | [X,Y]=meshgrid(xmin:cell:xmin+(n-1)*cell); 30 | 31 | X = reshape(X,n^2,1); 32 | Y = reshape(Y,n^2,1); 33 | Z= [X Y]; 34 | 35 | %compute initial distribution. 36 | D1 = pdist2(Z, mu, 'mahalanobis', S); 37 | 38 | prob1 = exp(-1/2*reshape(D1.^2,n,n)); 39 | eta1 = 1./sum(sum(prob1,1),2); 40 | prob1 = eta1*prob1; 41 | 42 | figure; 43 | %plotting takes a while. 44 | plotdistrib3(xmin:cell:xmin+(n-1)*cell,xmin:cell:xmin+(n-1)*cell,prob1, cell); 45 | 46 | for ee=1:1 47 | 48 | %this should be vectorized probably.or using blockproc-like function. 49 | for ii=1:n 50 | for jj=1:n 51 | prob2(ii,jj) = sum(sum((D(n+1-jj:2*n-jj , n+1-ii:2*n-ii )).*prob1,1),2); 52 | end 53 | end 54 | 55 | eta = 1./sum(sum(prob2,1),2); 56 | prob2 = eta.* prob2; 57 | 58 | figure; 59 | plotdistrib3(xmin:cell:xmin+(n-1)*cell,xmin:cell:xmin+(n-1)*cell,prob2, cell); 60 | prob1 = prob2; 61 | end 62 | 63 | %measurement update. 64 | z = 0.65; 65 | Q = 0.01; 66 | N = exp(-1/(2*Q)*((xmin:cell:xmin+(n-1)*cell)-z).^2); 67 | 68 | prob1 = bsxfun(@times, N', prob1); 69 | eta = 1./sum(sum(prob1,1),2); 70 | prob1 = eta.* prob1; 71 | 72 | figure; 73 | plotdistrib3(xmin:cell:xmin+(n-1)*cell,xmin:cell:xmin+(n-1)*cell,prob1, cell); 74 | -------------------------------------------------------------------------------- /ch4_nonparametric_filters/particle_filter_1.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | %Implementation of particle filter algorithm. 3 | %written by Pierre-Paul TACHER (ptacher@gmail.com). 4 | % ============================================================== 5 | 6 | %motion model matrix. 7 | A = [1 1;0 1]; 8 | 9 | %size of square grid. 10 | n = 200; 11 | 12 | %noise covariance. tweaked to avoid singularity. 13 | R = [1/4 1/2;1/2 1.001]; 14 | mu=[ 0 0]; 15 | 16 | cell = 0.2; 17 | [xmin ymin] = deal(-20, -20); 18 | 19 | [x1,x2] = meshgrid(xmin:cell:xmin+(n-1)*cell); 20 | f = zeros(n, n); 21 | 22 | %number of particles 23 | M = 10000; 24 | 25 | %particles 26 | X = zeros(M, 2); 27 | 28 | %weights (importance factors) 29 | w = ones(M, 1); 30 | 31 | for ii=1:5 32 | %sampling from conditional probabilities N(A*xt-1, R) 33 | X = X*A' + mvnrnd(mu, R, M); 34 | 35 | figure; 36 | axis([-20,20,-15,15]); 37 | hold on; 38 | set(gca,'XLimMode','manual','YLimMode','manual','DataAspectRatio',[1,1,1]); 39 | set(gca, 'XAxisLocation', 'origin', 'YAxisLocation', 'origin', 'Box', 'off'); 40 | plot(gca, X(:,1), X(:,2), '.','MarkerSize', 2); 41 | hold off; 42 | 43 | %Epanechnikov kernel density estimation 44 | %f = epankde(X, xmin:cell:xmin+(n-1)*cell, 3); 45 | 46 | %figure; 47 | %surf(gca, x1, x2, f); 48 | 49 | drawnow; 50 | 51 | end 52 | 53 | %low variance resampling. 54 | z = 5; 55 | Q = 10; 56 | 57 | N = exp(-1/(2*Q)*((X(:,1)-z).^2)); 58 | w= w.*N; 59 | 60 | bins = cumsum(w'*1/sum(w,1)); 61 | [~,~,index] = histcounts(1/M*rand + (0:1/M:1-1/M), bins); 62 | X = X(index+1,:); 63 | w = w(index+1); 64 | 65 | figure; 66 | axis([-20,20,-15,15]); 67 | hold on; 68 | set(gca,'XLimMode','manual','YLimMode','manual','DataAspectRatio',[1,1,1]); 69 | set(gca, 'XAxisLocation', 'origin', 'YAxisLocation', 'origin', 'Box', 'off'); 70 | plot(gca, X(:,1), X(:,2), '.', 'MarkerSize', 2); 71 | hold off; 72 | 73 | f = epankde(X, xmin:cell:xmin+(n-1)*cell, 3); 74 | 75 | figure; 76 | surf(gca, x1, x2, f); 77 | 78 | -------------------------------------------------------------------------------- /ch4_nonparametric_filters/particle_filter_2.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | %Implementation of particle filter algorithm. 3 | %written by Pierre-Paul TACHER (ptacher@gmail.com). 4 | % ============================================================== 5 | 6 | cell = 0.015; 7 | [xmin ymin] = deal(-1.5, -1.5); 8 | 9 | %size of square grid. 10 | n = 200; 11 | 12 | [x1,x2] = meshgrid(xmin:cell:xmin+(n-1)*cell); 13 | f = zeros(n, n); 14 | 15 | %initial , mean, covariance matrix 16 | S = [0.01 0;0 0.01]; 17 | mu=[0 0]; 18 | 19 | %variance of theta. 20 | s1 = 10000; 21 | 22 | %number of particles 23 | M = 10000; 24 | 25 | %particles 26 | X = zeros(M, 2); 27 | 28 | %weights (importance factors) 29 | w = ones(M, 1); 30 | 31 | %sampling from conditional probabilities p(xt | xt-1) 32 | X = simmov(S,s1,M); 33 | 34 | figure; 35 | axis([-1.5,1.5,-1.5,1.5]); 36 | hold on; 37 | set(gca,'XLimMode','manual','YLimMode','manual','DataAspectRatio',[1,1,1]); 38 | set(gca, 'XAxisLocation', 'origin', 'YAxisLocation', 'origin', 'Box', 'off'); 39 | plot(gca, X(:,1), X(:,2), '.','MarkerSize', 2); 40 | hold off; 41 | 42 | %Epanechnikov kernel density estimation 43 | f = epankde(X, xmin:cell:xmin+(n-1)*cell, 0.3); 44 | 45 | figure; 46 | axis([-1.5,1.5,-1.5,1.5,0 inf]); 47 | hold on; 48 | set(gca,'XLimMode','manual','YLimMode','manual','ZLimMode','auto','DataAspectRatio',[1,1,1]); 49 | surf(gca, x1, x2, f); 50 | drawnow; 51 | 52 | %low variance resampling. 53 | z = 0.65; 54 | Q = 0.01; 55 | 56 | N = exp(-1/(2*Q)*((X(:,1)-z).^2)); 57 | w= w.*N; 58 | 59 | bins = cumsum(w'*1/sum(w,1)); 60 | [~,~,index] = histcounts(1/M*rand + (0:1/M:1-1/M), bins); 61 | X = X(index+1,:); 62 | w = w(index+1); 63 | 64 | figure; 65 | axis([-1.5,1.5,-1.5,1.5]); 66 | hold on; 67 | set(gca,'XLimMode','manual','YLimMode','manual','DataAspectRatio',[1,1,1]); 68 | set(gca, 'XAxisLocation', 'origin', 'YAxisLocation', 'origin', 'Box', 'off'); 69 | plot(gca, X(:,1), X(:,2), '.', 'MarkerSize', 2); 70 | hold off; 71 | 72 | f = epankde(X, xmin:cell:xmin+(n-1)*cell, 0.3); 73 | 74 | figure; 75 | axis([-1.5,1.5,-1.5,1.5,0 inf]); 76 | hold on; 77 | set(gca,'XLimMode','manual','YLimMode','manual','ZLimMode','auto','DataAspectRatio',[1,1,1]); 78 | %surf(gca, x1, x2, f); 79 | 80 | -------------------------------------------------------------------------------- /ch4_nonparametric_filters/plotdistrib3.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %plot 3D histogram representing joint probability distibution 4 | %inspired from references: 5 | 6 | %scatterbar3 7 | % 8 | 9 | %Objects creation performance 10 | % 11 | 12 | %Note 'patch' function is called only once. 13 | 14 | % ============================================================== 15 | 16 | function plotdistrib3(X, Y, Z, width) 17 | 18 | i=0; 19 | [r,c]=size(Z); 20 | verts=zeros(8*r,3); 21 | col=zeros(8*r,1); 22 | faces=zeros(6*r,4); 23 | for j=1:r, 24 | x = X(j); 25 | for k=1:c, 26 | if ~isnan(Z(j,k)) 27 | y=Y(k);z=Z(j,k); 28 | faces(1+6*i:6*i+6,:) = [1 2 3 4;2 5 6 3;5 7 8 6;7 1 4 8;1 2 5 7;4 3 6 8]+8*i; 29 | w = width/2; 30 | verts(1+8*i:8*i+8,:) = [x-w y-w 0; x+w y-w 0;x+w y-w z; x-w y-w z;x+w y+w 0;x+w y+w z;x-w y+w 0;x-w y+w z]; 31 | col(1+8*i:8*i+8,:)= [0;0;z;z;0;z;0;z]; 32 | i=i+1; 33 | end 34 | end 35 | end 36 | 37 | patch('Vertices', verts, 'Faces', faces, 'FaceVertexCData',col,'FaceColor','interp', 'LineWidth', 0.1); 38 | zlim=[min(Z(:)) min(max(Z(:))*1.5,1)]; 39 | axis([min(X(:))-width max(X(:))+width min(Y(:))-width max(Y(:))+width zlim]) 40 | caxis([min(Z(:)) max(Z(:))]) 41 | xlabel('x'); ylabel('y'); zlabel('probability density'); 42 | view(15,40); 43 | drawnow; -------------------------------------------------------------------------------- /ch4_nonparametric_filters/simmov.m: -------------------------------------------------------------------------------- 1 | %simulation of random variable (x+cos?, y+sin?) 2 | function r = simmov(s1, s2, n) 3 | theta = normrnd(0, sqrt(s2), [n,1]) 4 | xy = mvnrnd([0 0], s1*eye(2), n) 5 | r = xy + [cos(theta) sin(theta)] 6 | end 7 | -------------------------------------------------------------------------------- /ch5_robot_motion/bicyclespl.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %sample bicyle motion model 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %X1: current state 7 | %alpha, v: commanded steering and forward velocity 8 | %n: number of samples 9 | %t: sample time interval 10 | 11 | %X: n x 3 samples matrix. 12 | 13 | % ============================================================== 14 | function [ X ] = bicyclespl(X1, alpha, v, t, n) 15 | X = zeros(n,3); 16 | [x, y, theta] = deal(X1(1,1), X1(1,2), X1(1,3)); 17 | 18 | sigma1 = deg2rad(15); 19 | sigma2 = 0.2; 20 | l = 1; 21 | 22 | alphas = max(min(normrnd(deg2rad(alpha), sigma1, [n,1]),pi*80/180),-pi*80/180); 23 | alphas = tan(alphas); 24 | vs = normrnd(v, sigma2*v, [n,1]); 25 | 26 | X = bsxfun(@plus, X, alphas); 27 | X = bsxfun(@times, X, [-sin(theta) cos(theta) 1]); 28 | X = bsxfun(@plus, X, [cos(theta) sin(theta) 0]); 29 | X = X .* bsxfun(@times,vs, t*[1 1 1/l]); 30 | X = bsxfun(@plus, X, [x, y, theta]); 31 | 32 | end 33 | 34 | -------------------------------------------------------------------------------- /ch5_robot_motion/ch5_robot_motion.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch5_robot_motion/ch5_robot_motion.pdf -------------------------------------------------------------------------------- /ch6_robot_perception/ch6_robot_perception.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch6_robot_perception/ch6_robot_perception.pdf -------------------------------------------------------------------------------- /ch6_robot_perception/robotposespl.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %sample robot pose from landmark 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %Xi: coordinates of image 7 | %Xm= coordinates of landmark 8 | %n: number of samples 9 | %s: variance noise 10 | %r: h/f ratio 11 | 12 | %X: n x 3 samples matrix. 13 | 14 | % ============================================================== 15 | function [ X ] = robotposepl(Xi, Xm, s, r, n) 16 | X = zeros(n,3); 17 | [xi, yi, thetai] = deal(Xi(1,1), Xi(1,2), Xi(1,3)); 18 | [xm, ym, thetam] = deal(Xm(1,1), Xm(1,2), Xm(1,3)); 19 | 20 | thetas = thetam-thetai-unidrnd(4,n,1)*pi/2; 21 | C = cos(thetas);S=sin(thetas); 22 | X(:,1:2) = -r*[xi*C-yi*S xi*S+yi*C]; 23 | X = bsxfun(@plus, X, [xm ym 0]); 24 | X(:,1:2) = X(:,1:2) + mvnrnd([0 0], [s 0;0 s], n); 25 | X(:,3) = thetas; 26 | 27 | end -------------------------------------------------------------------------------- /ch7_mrl_markov_and_gaussian/ch7_mrl_markov_and_gaussian.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch7_mrl_markov_and_gaussian/ch7_mrl_markov_and_gaussian.pdf -------------------------------------------------------------------------------- /ch7_mrl_markov_and_gaussian/ekf.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | % E Kalman filter algorithm localization implementation 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %mu: initial mean 7 | %S: initial covariance 8 | %v: commands n x 3 9 | %z: measurements n x k 10 | %t: time step 11 | 12 | %X: n x 3 means 13 | %Sigma: 3 x 3 x n covariance matrices 14 | 15 | % ============================================================== 16 | function [ X, Sigma ] = EKF(mu, S, v, z, t) 17 | n = size(v,1);%number of steps 18 | 19 | X = zeros(n+1,3); 20 | X(1,:) = mu; 21 | k = size(z,2); 22 | Sigma = zeros(3,3,n+1); 23 | Sigma(:,:,1) = S; 24 | s=0.01*eye(3);%motion noise variance 25 | K = zeros(3,1); 26 | B = [-10 -10 -10;10 10 -10;2 3 5;2 -4 8];%beacons 27 | 28 | V = eye(3); 29 | G = eye(3); 30 | Q = 0.001; 31 | 32 | H = zeros(k,3); 33 | 34 | for ii=2:n+1 35 | %motion update 36 | X(ii,:) = X(ii-1,:) + v(ii-1,:)*t; 37 | Sigma(:,:,ii) = Sigma(:,:,ii-1) + s; 38 | 39 | %measurement update 40 | H = bsxfun(@minus, X(ii,:), B); 41 | q = sqrt(sum(H.^2,2)); 42 | H = bsxfun(@times, 1./q, H); 43 | for jj = 1:k 44 | K = Sigma(:,:,ii) *H(jj,:)'* inv(H(jj,:)*Sigma(:,:,ii)*H(jj,:)'+Q); 45 | X(ii,:) = X(ii,:) + K'*(z(ii-1,jj)-q(jj)); 46 | Sigma(:,:,ii) = (eye(3) - K*H(jj,:))*Sigma(:,:,ii); 47 | end 48 | end 49 | 50 | X = X(2:n+1,:); 51 | Sigma = Sigma(:,:,2:n+1); 52 | end -------------------------------------------------------------------------------- /ch7_mrl_markov_and_gaussian/simulation.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %simulate motion and sensing of the simplistic underwater robot 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %x0: start position 7 | %t: time step 8 | %v: speed commands n x 3 9 | 10 | %X: n x 3 positions 11 | %Z: n x k beacons sensing 12 | 13 | % ============================================================== 14 | function [ X, Z ] = simulation(x0, t, v) 15 | n = size(v,1); 16 | B = [-10 -10 -10;10 10 -10;2 3 5;2 -4 8];%beacons 17 | 18 | X = zeros(n+1,3); 19 | X(1,:) = x0; 20 | k = size(B,1); 21 | Z = zeros(n+1,4); 22 | s=0.01;%motion noise variance 23 | 24 | for ii=2:n+1 25 | X(ii,:) = X(ii-1,:) + v(ii-1,:)*t + mvnrnd([0 0 0], s*eye(3)); 26 | Z(ii,:) = sqrt(sum((bsxfun(@minus, X(ii,:), B)).^2, 2))'; 27 | end 28 | 29 | X = X(2:n+1,:); 30 | Z = Z(2:n+1,:); 31 | end -------------------------------------------------------------------------------- /ch8_mrl_grid_and_monte_carlo/KLdivergence.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %compute Kullback?Leibler divergence from 4 | % discrete probability distribution q to p. 5 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 6 | 7 | % ============================================================== 8 | function [ KL ] = KLdivergence(p,q) 9 | 10 | KL = sum(p.*log2(p./q),2); 11 | 12 | end -------------------------------------------------------------------------------- /ch8_mrl_grid_and_monte_carlo/ch8_mrl_grid_and_monte_carlo.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pptacher/probabilistic_robotics/84bbaf4631aab84908526d9d74e02c39928d5266/ch8_mrl_grid_and_monte_carlo/ch8_mrl_grid_and_monte_carlo.pdf -------------------------------------------------------------------------------- /ch8_mrl_grid_and_monte_carlo/histogram_filter_3.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | %Implementation of histogram filter algorithm. 3 | %written by Pierre-Paul TACHER (ptacher@gmail.com). 4 | % ============================================================== 5 | 6 | %motion model matrix. 7 | A = eye(3); 8 | 9 | deltat = 0.1;%time step 10 | t=0; 11 | B = [-10 -10 -10;10 10 -10;2 3 5;2 -4 8];%beacons 12 | 13 | epsilon = 1e-7; 14 | 15 | %initial , mean, covariance matrix 16 | S = 0.001*eye(3); 17 | mu=[0 0 0]; 18 | 19 | %noise covariance. tweaked to avoid singularity. 20 | R = 0.1*eye(3); 21 | R1 = inv(R); 22 | d = det(R); 23 | 24 | cell = 0.025; 25 | [xmin ymin] = deal(-0.25, -0.25); 26 | 27 | %size of square grid. 28 | n = 100; 29 | 30 | prob1 = zeros(n, n, n); 31 | prob2 = zeros(n, n, n); 32 | prob3 = zeros(n,n); 33 | 34 | %precompute transition probabilities for gaussian noise. 35 | %requires (2n-1)^3 probabilities. needs to be done only once. 36 | [X1,Y1,Z1] = ndgrid(-(n-1)*cell:cell:(n-1)*cell); 37 | X1 = reshape(X1,(2*n-1)^3,1); 38 | Y1 = reshape(Y1,(2*n-1)^3,1); 39 | Z1 = reshape(Z1,(2*n-1)^3,1); 40 | T1= [X1 Y1 Z1]; 41 | 42 | [P,D]=eig(R1); 43 | U = sqrt(D)*P'; 44 | DD=sum((T1*U').^2, 2); 45 | D = zeros(2*n-1, 2*n-1, 2*n-1); 46 | D = exp(-1/2*(reshape(DD,2*n-1,2*n-1, 2*n-1))); 47 | 48 | %generate coordinates of cell centers. 49 | [X,Y,Z]=ndgrid(xmin:cell:xmin+(n-1)*cell); 50 | 51 | X = reshape(X,n^3,1); 52 | Y = reshape(Y,n^3,1); 53 | Z = reshape(Z,n^3,1); 54 | T= [X Y Z]; 55 | 56 | %compute initial distribution. 57 | D1 = pdist2(T, mu, 'mahalanobis', S); 58 | 59 | prob1 = exp(-1/2*reshape(D1.^2,n,n,n)); 60 | eta1 = 1./sum(sum(sum(prob1,1),2), 3); 61 | prob1 = eta1*prob1; 62 | 63 | %figure; 64 | %plotting takes a while. 65 | %plotdistrib3(xmin:cell:xmin+(n-1)*cell,xmin:cell:xmin+(n-1)*cell,prob1, cell); 66 | 67 | for ee=1:100 68 | %motion step 69 | t = t + deltat; 70 | v = [cos(0.6*t) sin(0.6*t) 0.1]; 71 | indexmap = @(l) max(min(l-sum(floor(deltat/cell.*v).*[1 n n^2],2),n^3), 1); 72 | 73 | %move probability masses according to X -> AX + v*deltat affine map. 74 | prob1 = prob1(indexmap(reshape(1:n^3, n, n, n))); 75 | 76 | eta = 0; 77 | %this should be vectorized probably.or using blockproc-like function. 78 | for ii=1:n 79 | for jj=1:n 80 | for kk=1:n 81 | if prob1(ii,jj,kk) >= epsilon 82 | prob2(ii,jj,kk) = sum(sum(sum((D(n+1-ii:2*n-ii , n+1-jj:2*n-jj , n+1-kk:2*n-kk)).*prob1))); 83 | else 84 | prob2(ii,jj,kk)= prob1(ii,jj,kk); 85 | end 86 | end 87 | end 88 | end 89 | 90 | eta = sum(sum(sum(prob2))); 91 | prob2 = 1/eta.* prob2; 92 | 93 | if mod(ee,10)==0 || ee==1 94 | figure; 95 | prob3 = sum(prob2,3); 96 | plotdistrib3(xmin:cell:xmin+(n-1)*cell,xmin:cell:xmin+(n-1)*cell,prob3, cell); 97 | end 98 | prob1 = prob2; 99 | end 100 | 101 | %measurement update. 102 | %todo 103 | 104 | %figure; 105 | %plotdistrib3(xmin:cell:xmin+(n-1)*cell,xmin:cell:xmin+(n-1)*cell,prob1, cell); 106 | -------------------------------------------------------------------------------- /ch8_mrl_grid_and_monte_carlo/plotdistrib3.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %plot 3D histogram representing joint probability distibution 4 | %inspired from references: 5 | 6 | %scatterbar3 7 | % 8 | 9 | %Objects creation performance 10 | % 11 | 12 | %Note 'patch' function is called only once. 13 | 14 | % ============================================================== 15 | 16 | function plotdistrib3(X, Y, Z, width) 17 | 18 | i=0; 19 | [r,c]=size(Z); 20 | verts=zeros(8*r,3); 21 | col=zeros(8*r,1); 22 | faces=zeros(6*r,4); 23 | for j=1:r, 24 | x = X(j); 25 | for k=1:c, 26 | if ~isnan(Z(j,k)) 27 | y=Y(k);z=Z(j,k); 28 | faces(1+6*i:6*i+6,:) = [1 2 3 4;2 5 6 3;5 7 8 6;7 1 4 8;1 2 5 7;4 3 6 8]+8*i; 29 | w = width/2; 30 | verts(1+8*i:8*i+8,:) = [x-w y-w 0; x+w y-w 0;x+w y-w z; x-w y-w z;x+w y+w 0;x+w y+w z;x-w y+w 0;x-w y+w z]; 31 | col(1+8*i:8*i+8,:)= [0;0;z;z;0;z;0;z]; 32 | i=i+1; 33 | end 34 | end 35 | end 36 | 37 | patch('Vertices', verts, 'Faces', faces, 'FaceVertexCData',col,'FaceColor','interp', 'LineWidth', 0.1); 38 | zlim=[min(Z(:)) min(max(Z(:))*1.5,1)]; 39 | axis([min(X(:))-width max(X(:))+width min(Y(:))-width max(Y(:))+width zlim]) 40 | caxis([min(Z(:)) max(Z(:))]) 41 | xlabel('x'); ylabel('y'); zlabel('probability density'); 42 | view(15,40); 43 | drawnow; -------------------------------------------------------------------------------- /ch8_mrl_grid_and_monte_carlo/post_resampling_dist.m: -------------------------------------------------------------------------------- 1 | % ============================================================== 2 | 3 | %compute probability distribution of particle location after resampling 4 | %written by Pierre-Paul TACHER (ptacher@gmail.com) 5 | 6 | %N: number of particles 7 | %p: 1 by 4 multinomial probabilities (k bins). must sum to 1. 8 | %z: 1 by 4 measurement model probabilities p(z | x ). 9 | 10 | %P: 1 by 4 by 2 vectors containing distributions P(X | z ) and P( X | not z). 11 | 12 | % ============================================================== 13 | function [ P1 ] = post_resampling_dist(N,p,z) 14 | 15 | [X1,X2,X3] = ndgrid(0:N); 16 | 17 | X4 = N - (X1+X2+X3); 18 | 19 | X1 = X1(:);X2=X2(:);X3=X3(:);X4=X4(:); 20 | %filter out vectors not in DN 21 | ind = X4 >= 0; 22 | X1 = X1(ind);X2=X2(ind);X3=X3(ind);X4=X4(ind); 23 | 24 | Y = mnpdf([X1,X2,X3,X4],repmat(p,size(X4,1),1)); 25 | 26 | 27 | Z=zeros(1,4,2); 28 | Z(1,:,1)= z; 29 | Z(1,:,2)= 1-z; 30 | 31 | X = bsxfun(@times, [X1, X2, X3, X4], Z); 32 | eta = sum(X,2); 33 | eta = 1./eta; 34 | 35 | X = bsxfun(@times, bsxfun(@times,Y,eta), X); 36 | P1 = sum(X,1); 37 | end --------------------------------------------------------------------------------