├── CPPFiles
    ├── InvertMatrix.hpp
    ├── cholesky.hpp
    ├── cppUKFData.txt
    ├── fmUKF.cpp
    ├── inputData.cpp
    ├── outputDataUKF.cpp
    └── quadStateEst.cpp
├── Data
    ├── poseNPPOrient.txt
    ├── poseNPPPos.txt
    ├── processInputAcc.txt
    ├── processInputOmega.txt
    ├── quadData.mat
    ├── viconEuler.txt
    ├── viconPos.txt
    └── viconTime.txt
├── MatlabFiles
    ├── MatlabEquations
    │   ├── 14GEqn.mat
    │   ├── 14GFunc.mat
    │   ├── 14LFunc.mat
    │   ├── imuGEqn.mat
    │   ├── imuGFunc.mat
    │   └── imuLFunc.mat
    └── MatlabSource
    │   ├── RPYToRotMat.m
    │   ├── RotMatToRPY.m
    │   ├── atan3.m
    │   ├── calculateCovEquation.m
    │   ├── final650.m
    │   ├── imuUKF.m
    │   ├── nPointPose.m
    │   ├── outputSensorLog.m
    │   ├── plotUKFcppData.m
    │   ├── quadUKF.m
    │   └── symbolicFuncs.m
├── README.md
├── presentationKF.pdf
└── ukfROS
    ├── InvertMatrix.hpp
    ├── cholesky.hpp
    ├── fmUKF.cpp
    ├── inputData.cpp
    ├── quadStateEstROS
    └── quadStateEstROS.cpp


/CPPFiles/InvertMatrix.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef INVERT_MATRIX_HPP
 2 | #define INVERT_MATRIX_HPP
 3 | 
 4 | /*
 5 |  * Modified from http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
 6 |  * By, Fredrik Orderud.
 7 |  */
 8 | 
 9 | 
10 | // REMEMBER to update "lu.hpp" header includes from boost-CVS
11 | //#include <boost/numeric/ublas/vector.hpp>
12 | //#include <boost/numeric/ublas/vector_proxy.hpp>
13 | //#include <boost/numeric/ublas/matrix.hpp>
14 | //#include <boost/numeric/ublas/triangular.hpp>
15 | #include <boost/numeric/ublas/lu.hpp>
16 | //#include <boost/numeric/ublas/io.hpp>
17 | 
18 | //namespace ublas = boost::numeric::ublas;
19 | 
20 | /* Matrix inversion routine.
21 | Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
22 | template<class T>
23 | bool InvertMatrix (const boost::numeric::ublas::matrix<T>& input,boost::numeric::ublas::matrix<T>& inverse) {
24 | //using namespace boost::numeric::ublas;
25 | 
26 | typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;
27 | 
28 | // create a working copy of the input
29 | boost::numeric::ublas::matrix<T> A(input);
30 | 
31 | // create a permutation matrix for the LU-factorization
32 | pmatrix pm(A.size1());
33 | 
34 | // perform LU-factorization
35 | int res = lu_factorize(A,pm);
36 |     if( res != 0 ) return false;
37 | 
38 | // create identity matrix of "inverse"
39 | inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));
40 | 
41 | // backsubstitute to get the inverse
42 | lu_substitute(A, pm, inverse);
43 | return true;
44 | }
45 | #endif //INVERT_MATRIX_HPP
46 | 


--------------------------------------------------------------------------------
/CPPFiles/cholesky.hpp:
--------------------------------------------------------------------------------
  1 | /** -*- c++ -*- \file cholesky.hpp \brief cholesky decomposition */
  2 | /*
  3 |  -   begin                : 2005-08-24
  4 |  -   copyright            : (C) 2005 by Gunter Winkler, Konstantin Kutzkow
  5 |  -   email                : guwi17@gmx.de
  6 | 
  7 |     This library is free software; you can redistribute it and/or
  8 |     modify it under the terms of the GNU Lesser General Public
  9 |     License as published by the Free Software Foundation; either
 10 |     version 2.1 of the License, or (at your option) any later version.
 11 | 
 12 |     This library is distributed in the hope that it will be useful,
 13 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 14 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 15 |     Lesser General Public License for more details.
 16 | 
 17 |     You should have received a copy of the GNU Lesser General Public
 18 |     License along with this library; if not, write to the Free Software
 19 |     Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 20 | 
 21 | */
 22 | 
 23 | #ifndef _H_CHOLESKY_HPP_
 24 | #define _H_CHOLESKY_HPP_
 25 | 
 26 | 
 27 | #include <cassert>
 28 | 
 29 | #include <boost/numeric/ublas/vector.hpp>
 30 | #include <boost/numeric/ublas/vector_proxy.hpp>
 31 | 
 32 | #include <boost/numeric/ublas/matrix.hpp>
 33 | #include <boost/numeric/ublas/matrix_proxy.hpp>
 34 | 
 35 | #include <boost/numeric/ublas/vector_expression.hpp>
 36 | #include <boost/numeric/ublas/matrix_expression.hpp>
 37 | 
 38 | #include <boost/numeric/ublas/triangular.hpp>
 39 | 
 40 | namespace ublasCHOL = boost::numeric::ublas;
 41 | 
 42 | 
 43 | /** \brief decompose the symmetric positive definit matrix A into product L L^T.
 44 |  *
 45 |  * \param MATRIX type of input matrix 
 46 |  * \param TRIA type of lower triangular output matrix
 47 |  * \param A square symmetric positive definite input matrix (only the lower triangle is accessed)
 48 |  * \param L lower triangular output matrix 
 49 |  * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
 50 |  */
 51 | template < class MATRIX, class TRIA >
 52 | size_t cholesky_decompose(const MATRIX& A, TRIA& L)
 53 | {
 54 |   using namespace ublasCHOL;
 55 | 
 56 |   typedef typename MATRIX::value_type T;
 57 |   
 58 |   assert( A.size1() == A.size2() );
 59 |   assert( A.size1() == L.size1() );
 60 |   assert( A.size2() == L.size2() );
 61 | 
 62 |   const size_t n = A.size1();
 63 |   
 64 |   for (size_t k=0 ; k < n; k++) {
 65 |         
 66 |     double qL_kk = A(k,k) - inner_prod( project( row(L, k), range(0, k) ),
 67 |                                         project( row(L, k), range(0, k) ) );
 68 |     
 69 |     if (qL_kk <= 0) {
 70 |       return 1 + k;
 71 |     } else {
 72 |       double L_kk = sqrt( qL_kk );
 73 |       L(k,k) = L_kk;
 74 |       
 75 |       matrix_column<TRIA> cLk(L, k);
 76 |       project( cLk, range(k+1, n) )
 77 |         = ( project( column(A, k), range(k+1, n) )
 78 |             - prod( project(L, range(k+1, n), range(0, k)), 
 79 |                     project(row(L, k), range(0, k) ) ) ) / L_kk;
 80 |     }
 81 |   }
 82 |   return 0;      
 83 | }
 84 | 
 85 | 
 86 | /** \brief decompose the symmetric positive definit matrix A into product L L^T.
 87 |  *
 88 |  * \param MATRIX type of matrix A
 89 |  * \param A input: square symmetric positive definite matrix (only the lower triangle is accessed)
 90 |  * \param A output: the lower triangle of A is replaced by the cholesky factor
 91 |  * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
 92 |  */
 93 | template < class MATRIX >
 94 | size_t cholesky_decompose(MATRIX& A)
 95 | {
 96 |   using namespace ublasCHOL;
 97 | 
 98 |   typedef typename MATRIX::value_type T;
 99 |   
100 |   const MATRIX& A_c(A);
101 | 
102 |   const size_t n = A.size1();
103 |   
104 |   for (size_t k=0 ; k < n; k++) {
105 |         
106 |     double qL_kk = A_c(k,k) - inner_prod( project( row(A_c, k), range(0, k) ),
107 |                                           project( row(A_c, k), range(0, k) ) );
108 |     
109 |     if (qL_kk <= 0) {
110 |       return 1 + k;
111 |     } else {
112 |       double L_kk = sqrt( qL_kk );
113 |       
114 |       matrix_column<MATRIX> cLk(A, k);
115 |       project( cLk, range(k+1, n) )
116 |         = ( project( column(A_c, k), range(k+1, n) )
117 |             - prod( project(A_c, range(k+1, n), range(0, k)), 
118 |                     project(row(A_c, k), range(0, k) ) ) ) / L_kk;
119 |       A(k,k) = L_kk;
120 |     }
121 |   }
122 |   return 0;      
123 | }
124 | 
125 | #if 0
126 |   using namespace ublasCHOL;
127 | 
128 |     // Operations:
129 |     //  n * (n - 1) / 2 + n = n * (n + 1) / 2 multiplications,
130 |     //  n * (n - 1) / 2 additions
131 | 
132 |     // Dense (proxy) case
133 |     template<class E1, class E2>
134 |     void inplace_solve (const matrix_expression<E1> &e1, vector_expression<E2> &e2,
135 |                         lower_tag, column_major_tag) {
136 |       std::cout << " is_lc ";
137 |         typedef typename E2::size_type size_type;
138 |         typedef typename E2::difference_type difference_type;
139 |         typedef typename E2::value_type value_type;
140 | 
141 |         BOOST_UBLAS_CHECK (e1 ().size1 () == e1 ().size2 (), bad_size ());
142 |         BOOST_UBLAS_CHECK (e1 ().size2 () == e2 ().size (), bad_size ());
143 |         size_type size = e2 ().size ();
144 |         for (size_type n = 0; n < size; ++ n) {
145 | #ifndef BOOST_UBLAS_SINGULAR_CHECK
146 |             BOOST_UBLAS_CHECK (e1 () (n, n) != value_type/*zero*/(), singular ());
147 | #else
148 |             if (e1 () (n, n) == value_type/*zero*/())
149 |                 singular ().raise ();
150 | #endif
151 |             value_type t = e2 () (n) / e1 () (n, n);
152 |             e2 () (n) = t;
153 |             if (t != value_type/*zero*/()) {
154 |               project( e2 (), range(n+1, size) )
155 |                 .minus_assign( t * project( column( e1 (), n), range(n+1, size) ) );
156 |             }
157 |         }
158 |     }
159 | #endif
160 | 
161 | 
162 | 
163 | 
164 | 
165 | /** \brief decompose the symmetric positive definit matrix A into product L L^T.
166 |  *
167 |  * \param MATRIX type of matrix A
168 |  * \param A input: square symmetric positive definite matrix (only the lower triangle is accessed)
169 |  * \param A output: the lower triangle of A is replaced by the cholesky factor
170 |  * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
171 |  */
172 | template < class MATRIX >
173 | size_t incomplete_cholesky_decompose(MATRIX& A)
174 | {
175 |   using namespace ublasCHOL;
176 | 
177 |   typedef typename MATRIX::value_type T;
178 |   
179 |   // read access to a const matrix is faster
180 |   const MATRIX& A_c(A);
181 | 
182 |   const size_t n = A.size1();
183 |   
184 |   for (size_t k=0 ; k < n; k++) {
185 |     
186 |     double qL_kk = A_c(k,k) - inner_prod( project( row( A_c, k ), range(0, k) ),
187 |                                           project( row( A_c, k ), range(0, k) ) );
188 |     
189 |     if (qL_kk <= 0) {
190 |       return 1 + k;
191 |     } else {
192 |       double L_kk = sqrt( qL_kk );
193 | 
194 |       // aktualisieren
195 |       for (size_t i = k+1; i < A.size1(); ++i) {
196 |         T* Aik = A.find_element(i, k);
197 | 
198 |         if (Aik != 0) {
199 |           *Aik = ( *Aik - inner_prod( project( row( A_c, k ), range(0, k) ),
200 |                                       project( row( A_c, i ), range(0, k) ) ) ) / L_kk;
201 |         }
202 |       }
203 |         
204 |       A(k,k) = L_kk;
205 |     }
206 |   }
207 |         
208 |   return 0;
209 | }
210 | 
211 | 
212 | 
213 | 
214 | /** \brief solve system L L^T x = b inplace
215 |  *
216 |  * \param L a triangular matrix
217 |  * \param x input: right hand side b; output: solution x
218 |  */
219 | template < class TRIA, class VEC >
220 | void
221 | cholesky_solve(const TRIA& L, VEC& x, ublasCHOL::lower)
222 | {
223 |   using namespace ublasCHOL;
224 | //   ::inplace_solve(L, x, lower_tag(), typename TRIA::orientation_category () );
225 |   inplace_solve(L, x, lower_tag() );
226 |   inplace_solve(trans(L), x, upper_tag());
227 | }
228 | 
229 | 
230 | #endif
231 | 


--------------------------------------------------------------------------------
/CPPFiles/cppUKFData.txt:
--------------------------------------------------------------------------------
  1 | -0.197613 0.0797738 0.873868 
  2 | -0.2036 0.087925 0.87599 
  3 | -0.211833 0.0958052 0.878084 
  4 | -0.209376 0.0993442 0.878563 
  5 | -0.217803 0.107172 0.880476 
  6 | -0.212062 0.111899 0.881445 
  7 | -0.222234 0.120952 0.883437 
  8 | -0.223413 0.121981 0.88365 
  9 | -0.169083 0.126271 0.885779 
 10 | -0.185605 0.147749 0.885903 
 11 | -0.194418 0.156295 0.88715 
 12 | -0.164434 0.162277 0.883571 
 13 | -0.173972 0.171183 0.884563 
 14 | -0.17707 0.174027 0.884841 
 15 | -0.12871 0.182912 0.880606 
 16 | -0.141033 0.194116 0.881228 
 17 | -0.14246 0.195399 0.881276 
 18 | -0.14754 0.199946 0.881405 
 19 | -0.154794 0.206384 0.881466 
 20 | -0.158644 0.209776 0.88144 
 21 | -0.128087 0.219858 0.875934 
 22 | -0.124882 0.239376 0.872075 
 23 | -0.119132 0.254478 0.869019 
 24 | -0.132271 0.266206 0.868045 
 25 | -0.112948 0.272056 0.865928 
 26 | -0.109697 0.288262 0.863364 
 27 | -0.109108 0.305448 0.860969 
 28 | -0.125544 0.320346 0.859339 
 29 | -0.127336 0.321942 0.859158 
 30 | -0.0999177 0.329553 0.857193 
 31 | -0.101219 0.35041 0.85406 
 32 | -0.0978965 0.369165 0.8516 
 33 | -0.117684 0.387119 0.849335 
 34 | -0.0964333 0.390554 0.848331 
 35 | -0.0950606 0.409521 0.845697 
 36 | -0.0962549 0.428604 0.843134 
 37 | -0.117931 0.44808 0.840938 
 38 | -0.120984 0.450753 0.840646 
 39 | -0.127401 0.456319 0.84004 
 40 | -0.136425 0.464035 0.839203 
 41 | -0.144662 0.470971 0.838449 
 42 | -0.0989248 0.474934 0.837116 
 43 | -0.0869416 0.496113 0.834469 
 44 | -0.079722 0.513887 0.832122 
 45 | -0.0741743 0.533277 0.829362 
 46 | -0.0762391 0.552974 0.826937 
 47 | -0.0760532 0.572984 0.824715 
 48 | -0.0750157 0.591693 0.822453 
 49 | -0.0748231 0.610402 0.820472 
 50 | -0.075553 0.628446 0.818719 
 51 | -0.0736822 0.645496 0.817444 
 52 | -0.0715797 0.663084 0.816693 
 53 | -0.0699811 0.680108 0.81637 
 54 | -0.0666023 0.695937 0.816417 
 55 | -0.0653824 0.711117 0.816824 
 56 | -0.0652003 0.725199 0.817628 
 57 | -0.093435 0.750342 0.819774 
 58 | -0.0690663 0.745842 0.819622 
 59 | -0.0729745 0.763497 0.822131 
 60 | -0.0996886 0.788134 0.82517 
 61 | -0.0709278 0.778351 0.8249 
 62 | -0.0682672 0.789456 0.82749 
 63 | -0.0665211 0.802013 0.83061 
 64 | -0.0659266 0.81426 0.833996 
 65 | -0.0669632 0.826519 0.837839 
 66 | -0.0698909 0.839022 0.842114 
 67 | -0.0713167 0.851605 0.84652 
 68 | -0.0736033 0.864161 0.851161 
 69 | -0.0961889 0.891151 0.857032 
 70 | -0.0782246 0.88039 0.857198 
 71 | -0.079657 0.892753 0.862514 
 72 | -0.0806554 0.903359 0.867663 
 73 | -0.100981 0.930685 0.874688 
 74 | -0.103101 0.933533 0.87544 
 75 | -0.110142 0.942987 0.877958 
 76 | -0.116483 0.951499 0.88025 
 77 | -0.0942662 0.934475 0.880908 
 78 | -0.0950616 0.946017 0.888483 
 79 | -0.0908931 0.951892 0.893829 
 80 | -0.106536 0.976615 0.901315 
 81 | -0.10834 0.97947 0.902194 
 82 | -0.0951113 0.968874 0.903022 
 83 | -0.0959836 0.981396 0.910852 
 84 | -0.0936586 0.990975 0.917482 
 85 | -0.0953263 1.00181 0.924668 
 86 | -0.0911151 1.0143 0.932277 
 87 | -0.0849578 1.02575 0.941095 
 88 | -0.0891563 1.03813 0.949284 
 89 | -0.0872314 1.04997 0.957574 
 90 | -0.0848797 1.06212 0.966355 
 91 | -0.092932 1.08747 0.977414 
 92 | -0.0937639 1.09015 0.978597 
 93 | -0.083096 1.08183 0.979472 
 94 | -0.0794633 1.0962 0.990505 
 95 | -0.074753 1.10502 0.99921 
 96 | -0.0783981 1.12764 1.00999 
 97 | -0.0698346 1.12055 1.01145 
 98 | -0.064021 1.13199 1.02175 
 99 | -0.0587271 1.14034 1.03079 
100 | -0.0531627 1.14866 1.03966 
101 | -0.0469079 1.15816 1.04911 
102 | -0.0452885 1.178 1.05948 
103 | -0.0374471 1.17557 1.06313 
104 | -0.0242754 1.19356 1.07697 
105 | -0.0218373 1.20222 1.08795 
106 | -0.00878742 1.21365 1.09873 
107 | 0.00143371 1.22623 1.11014 
108 | 0.0122613 1.24032 1.12259 
109 | 0.023196 1.2538 1.13516 
110 | 0.0343462 1.2672 1.1482 
111 | 0.0449472 1.28071 1.16154 
112 | 0.0559114 1.29437 1.17525 
113 | 0.0665065 1.30866 1.18966 
114 | 0.0775257 1.32286 1.20432 
115 | 0.0875172 1.33751 1.21959 
116 | 0.0952967 1.35068 1.23463 
117 | 0.106604 1.3712 1.24928 
118 | 0.107858 1.37348 1.25091 
119 | 0.113006 1.38285 1.25762 
120 | 0.1168 1.38975 1.26257 
121 | 0.112845 1.38161 1.26526 
122 | 0.123726 1.39585 1.28101 
123 | 0.137213 1.41803 1.29732 
124 | 0.138513 1.42016 1.29888 
125 | 0.142633 1.42691 1.30379 
126 | 0.136861 1.42207 1.30776 
127 | 0.146001 1.43551 1.32323 
128 | 0.153109 1.44638 1.33582 
129 | 0.169825 1.46911 1.35258 
130 | 0.171257 1.47103 1.35399 
131 | 0.176471 1.47801 1.35906 
132 | 0.166736 1.47148 1.36055 
133 | 0.177317 1.48594 1.37503 
134 | 0.185914 1.49892 1.38668 
135 | 0.196342 1.5121 1.39784 
136 | 0.205746 1.52565 1.40863 
137 | 0.225125 1.54566 1.42241 
138 | 0.220065 1.54698 1.42471 
139 | 0.24474 1.57133 1.44095 
140 | 0.248296 1.57479 1.4432 
141 | 0.256895 1.5831 1.44853 
142 | 0.262118 1.58811 1.4517 
143 | 0.269512 1.59514 1.45608 
144 | 0.254419 1.59192 1.45566 
145 | 0.262629 1.60562 1.46505 
146 | 0.270843 1.61846 1.47354 
147 | 0.283622 1.63265 1.48246 
148 | 0.305782 1.6505 1.49324 
149 | 0.297715 1.64923 1.49256 
150 | 0.311418 1.66422 1.50159 
151 | 0.321441 1.67663 1.50905 
152 | 0.330908 1.68922 1.51687 
153 | 0.340798 1.70157 1.5243 
154 | 0.35053 1.71618 1.53339 
155 | 0.361731 1.73088 1.54236 
156 | 0.373089 1.74455 1.55081 
157 | 0.382696 1.75742 1.5591 
158 | 0.394578 1.77062 1.56737 
159 | 0.414954 1.78262 1.57508 
160 | 0.407418 1.78545 1.57669 
161 | 0.428676 1.79738 1.58446 
162 | 0.423715 1.80168 1.58704 
163 | 0.436127 1.81517 1.59581 
164 | 0.447245 1.82685 1.60351 
165 | 0.462052 1.84103 1.61316 
166 | 0.475783 1.85434 1.62214 
167 | 0.490082 1.86667 1.6306 
168 | 0.504906 1.87798 1.63865 
169 | 0.526509 1.8913 1.64826 
170 | 0.541979 1.90131 1.65543 
171 | 0.561066 1.91163 1.66309 
172 | 0.57871 1.92042 1.66999 
173 | 0.593871 1.92634 1.67709 
174 | 0.60895 1.93223 1.68277 
175 | 0.624378 1.93859 1.68792 
176 | 0.643142 1.94401 1.69307 
177 | 0.664534 1.95005 1.69865 
178 | 0.686591 1.95396 1.7044 
179 | 0.708745 1.95719 1.71008 
180 | 0.731104 1.96056 1.71579 
181 | 0.756246 1.96242 1.72128 
182 | 0.782842 1.96312 1.72666 
183 | 0.806289 1.96261 1.73198 
184 | 0.830743 1.96136 1.73725 
185 | 0.85741 1.96019 1.74223 
186 | 0.882886 1.95844 1.74706 
187 | 0.919983 1.95296 1.75121 
188 | 0.933365 1.95092 1.75264 
189 | 0.913539 1.95054 1.75456 
190 | 0.962453 1.94171 1.75937 
191 | 0.965718 1.94109 1.75967 
192 | 0.957705 1.94784 1.75987 
193 | 1.00328 1.93898 1.76337 
194 | 1.0078 1.93809 1.76368 
195 | 1.02186 1.93532 1.76458 
196 | 1.08826 1.93052 1.76733 
197 | 1.13163 1.92478 1.76907 
198 | 1.13731 1.92403 1.76924 
199 | 1.00157 2.02792 1.87489 
200 | 0.986523 2.00946 1.8507 
201 | 1.01085 1.99404 1.83459 
202 | 1.03831 1.98125 1.82308 
203 | 1.06707 1.97013 1.81488 
204 | 1.0975 1.96081 1.80841 
205 | 1.12396 1.95276 1.80323 
206 | 1.15055 1.94569 1.79909 
207 | 1.17916 1.93972 1.79567 
208 | 1.20691 1.93485 1.79255 
209 | 1.23323 1.93095 1.78958 
210 | 1.27745 1.92723 1.78757 
211 | 1.32496 1.92624 1.78548 
212 | 1.36208 1.92631 1.78372 
213 | 1.39774 1.92729 1.78192 
214 | 1.44056 1.9275 1.78033 
215 | 1.4785 1.9263 1.77909 
216 | 1.50887 1.91843 1.77821 
217 | 1.51735 1.9161 1.77796 
218 | 1.53075 1.9123 1.77756 
219 | 1.54035 1.92092 1.77752 
220 | 1.57679 1.91825 1.77681 
221 | 1.61233 1.91633 1.77638 
222 | 1.64483 1.91308 1.77604 
223 | 1.67466 1.90826 1.77611 
224 | 1.70871 1.90292 1.77642 
225 | 1.73671 1.89814 1.77702 
226 | 1.76054 1.89364 1.77757 
227 | 1.78414 1.88798 1.77809 
228 | 1.80735 1.88292 1.7787 
229 | 1.83232 1.87702 1.77918 
230 | 1.85593 1.86923 1.77947 
231 | 1.87775 1.86048 1.78002 
232 | 1.89767 1.85234 1.7807 
233 | 1.92111 1.84536 1.78165 
234 | 1.94791 1.83008 1.78157 
235 | 1.95715 1.82467 1.78155 
236 | 1.96501 1.82 1.78152 
237 | 1.97137 1.81617 1.78148 
238 | 1.97574 1.82134 1.78312 
239 | 1.99857 1.80997 1.7841 
240 | 2.01367 1.8012 1.78492 
241 | 2.03507 1.79192 1.78533 
242 | 2.05295 1.78312 1.78581 
243 | 2.0675 1.77532 1.78597 
244 | 2.08056 1.76799 1.7857 
245 | 2.09564 1.75925 1.78487 
246 | 2.11064 1.74777 1.78372 
247 | 2.12313 1.73676 1.78245 
248 | 2.13375 1.72508 1.78128 
249 | 2.14182 1.71506 1.77996 
250 | 2.14784 1.70485 1.77846 
251 | 2.15347 1.69234 1.77687 
252 | 2.15718 1.68038 1.77514 
253 | 2.15859 1.6683 1.77355 
254 | 2.15788 1.65583 1.77218 
255 | 2.15737 1.6419 1.77045 
256 | 2.15676 1.6244 1.76969 
257 | 2.15665 1.62276 1.76962 
258 | 2.15632 1.61837 1.76944 
259 | 2.15495 1.6195 1.76767 
260 | 2.15222 1.60459 1.76509 
261 | 2.14879 1.58999 1.76273 
262 | 2.14359 1.57583 1.76024 
263 | 2.13675 1.56031 1.75752 
264 | 2.12892 1.54593 1.75501 
265 | 2.12167 1.53107 1.7523 
266 | 2.11392 1.51533 1.74965 
267 | 2.10454 1.49715 1.74608 
268 | 2.09279 1.47536 1.74206 
269 | 2.08115 1.45404 1.73801 
270 | 2.06973 1.42597 1.73527 
271 | 2.06782 1.42126 1.73479 
272 | 2.06796 1.42634 1.73281 
273 | 2.05898 1.40926 1.7289 
274 | 2.05233 1.39486 1.72558 
275 | 2.04557 1.37902 1.72237 
276 | 2.03673 1.36186 1.71888 
277 | 2.02684 1.34035 1.71487 
278 | 2.01758 1.31516 1.71002 
279 | 2.00256 1.2823 1.70618 
280 | 2.00103 1.27895 1.70579 
281 | 2.00491 1.28114 1.70405 
282 | 1.99401 1.25335 1.69912 
283 | 1.98325 1.22964 1.69477 
284 | 1.97459 1.2073 1.69061 
285 | 1.96349 1.18605 1.68635 
286 | 1.95424 1.16625 1.68206 
287 | 1.94108 1.1439 1.67741 
288 | 1.92511 1.1184 1.67296 
289 | 1.92417 1.11588 1.67139 
290 | 1.90761 1.09222 1.66602 
291 | 1.89339 1.07236 1.66094 
292 | 1.87825 1.05354 1.65616 
293 | 1.86035 1.03362 1.6508 
294 | 1.84391 1.01528 1.64569 
295 | 1.82683 0.996745 1.64102 
296 | 1.81172 0.979792 1.63648 
297 | 1.79296 0.961264 1.63147 
298 | 1.77597 0.944447 1.62663 
299 | 1.75629 0.928993 1.62204 
300 | 1.73833 0.914324 1.61762 
301 | 1.72139 0.90084 1.61328 
302 | 1.70458 0.888626 1.60931 
303 | 1.67874 0.869869 1.60478 
304 | 1.67144 0.864665 1.60352 
305 | 1.66356 0.859099 1.60217 
306 | 1.67387 0.865743 1.60214 
307 | 1.65598 0.853657 1.59742 
308 | 1.63921 0.844951 1.59405 
309 | 1.61758 0.834321 1.58956 
310 | 1.5841 0.816911 1.58454 
311 | 1.58151 0.815596 1.58415 
312 | 1.57077 0.810177 1.58256 
313 | 1.56265 0.806128 1.58134 
314 | 1.58285 0.815425 1.58187 
315 | 1.5661 0.808267 1.57774 
316 | 1.55014 0.80185 1.57416 
317 | 1.5263 0.793399 1.5695 
318 | 1.49623 0.783709 1.56398 
319 | 1.47076 0.77527 1.55868 
320 | 1.44038 0.766328 1.5545 
321 | 1.42511 0.762046 1.55245 
322 | 1.41809 0.760125 1.55152 
323 | 1.42211 0.760788 1.54994 
324 | 1.3979 0.754723 1.54469 
325 | 1.37385 0.749294 1.53968 
326 | 1.35109 0.744851 1.53513 
327 | 1.31236 0.738801 1.53039 
328 | 1.30583 0.737868 1.52961 
329 | 1.31118 0.738176 1.52781 
330 | 1.26517 0.732749 1.52243 
331 | 1.25759 0.731945 1.52157 
332 | 1.24887 0.731049 1.52059 
333 | 1.23328 0.729521 1.51885 
334 | 1.22218 0.72849 1.5176 
335 | 1.20331 0.72684 1.51548 
336 | 1.19552 0.726196 1.5146 
337 | 1.18767 0.725567 1.5137 
338 | 1.16959 0.724197 1.5116 
339 | 1.16165 0.723628 1.51066 
340 | 1.15369 0.723077 1.5097 
341 | 1.20755 0.726663 1.50918 
342 | 1.18784 0.727051 1.50437 
343 | 1.16732 0.727401 1.50043 
344 | 1.14084 0.727964 1.49623 
345 | 1.10619 0.728021 1.49141 
346 | 1.08121 0.727993 1.48771 
347 | 1.05581 0.728814 1.48381 
348 | 1.02865 0.730117 1.47972 
349 | 0.994385 0.731791 1.47504 
350 | 0.957851 0.734428 1.46985 
351 | 0.920289 0.73666 1.46499 
352 | 0.884637 0.73931 1.46038 
353 | 0.850301 0.741894 1.45607 
354 | 0.81756 0.745472 1.45175 
355 | 0.785451 0.749027 1.44754 
356 | 0.753307 0.753153 1.44332 
357 | 0.704426 0.762471 1.43789 
358 | 0.700421 0.763267 1.43744 
359 | 0.709322 0.759908 1.43745 
360 | 0.67528 0.765751 1.43285 
361 | 0.652674 0.770549 1.42952 
362 | 0.627033 0.776014 1.42559 
363 | 0.596889 0.783126 1.42106 
364 | 0.561038 0.792747 1.41569 
365 | 0.518063 0.804294 1.40928 
366 | 0.47949 0.815624 1.40306 
367 | 0.437878 0.828382 1.39604 
368 | 0.397725 0.842955 1.3882 
369 | 0.360851 0.856392 1.38107 
370 | 0.324099 0.871673 1.37304 
371 | 0.293971 0.885624 1.3657 
372 | 0.272507 0.897324 1.35934 
373 | 0.254115 0.909031 1.35327 
374 | 0.238748 0.921243 1.34728 
375 | 0.22406 0.934099 1.34099 
376 | 0.207948 0.948345 1.33417 
377 | 0.196602 0.962021 1.32761 
378 | 0.189106 0.975915 1.32088 
379 | 0.181913 0.990212 1.31403 
380 | 0.178212 1.00345 1.30752 
381 | 0.174791 1.01703 1.30097 
382 | 0.17409 1.03102 1.29439 
383 | 0.174428 1.04497 1.28787 
384 | 0.176402 1.0594 1.28135 
385 | 0.178134 1.07317 1.27524 
386 | 0.181857 1.08994 1.26779 
387 | 0.185898 1.1033 1.26172 
388 | 0.191665 1.12278 1.25368 
389 | 0.197367 1.1344 1.24902 
390 | 0.206583 1.14758 1.24401 
391 | 0.207172 1.17269 1.23578 
392 | 0.217402 1.16622 1.23757 
393 | 0.229264 1.18273 1.2322 
394 | 0.239445 1.19942 1.22709 
395 | 0.25112 1.21688 1.22202 
396 | 0.262373 1.23366 1.21748 
397 | 0.272648 1.25046 1.2133 
398 | 0.286851 1.27078 1.20827 
399 | 0.30178 1.29071 1.2037 
400 | 0.316235 1.31079 1.19936 
401 | 0.329645 1.3298 1.19559 
402 | 0.343272 1.34889 1.19215 
403 | 0.355892 1.3628 1.18997 
404 | 0.371577 1.38455 1.18671 
405 | 0.384747 1.39671 1.1849 
406 | 0.398691 1.412 1.18262 
407 | 0.413099 1.43018 1.18002 
408 | 0.430282 1.45287 1.17694 
409 | 0.446176 1.47459 1.17454 
410 | 0.465526 1.51032 1.17091 
411 | 0.46918 1.51712 1.17026 
412 | 0.471042 1.51201 1.17148 
413 | 0.489092 1.53636 1.1702 
414 | 0.501125 1.55366 1.16969 
415 | 0.512314 1.57174 1.16953 
416 | 0.523735 1.59094 1.16972 
417 | 0.534526 1.61167 1.17037 
418 | 0.5467 1.63387 1.17116 
419 | 0.550683 1.64887 1.17216 
420 | 0.556939 1.66503 1.17353 
421 | 0.573603 1.69445 1.17437 
422 | 0.578433 1.7031 1.17468 
423 | 0.568546 1.69431 1.17572 
424 | 0.577734 1.71791 1.17777 
425 | 0.585651 1.73556 1.17955 
426 | 0.593302 1.75328 1.18157 
427 | 0.60161 1.77534 1.18401 
428 | 0.61029 1.79792 1.1868 
429 | 0.618434 1.81916 1.18973 
430 | 0.628741 1.8446 1.19372 
431 | 0.638629 1.86652 1.19753 
432 | 0.647653 1.8888 1.20179 
433 | 0.657606 1.91055 1.2066 
434 | 0.663357 1.93097 1.21108 
435 | 0.669582 1.94863 1.21498 
436 | 0.676645 1.96706 1.21926 
437 | 0.699879 1.99391 1.22606 
438 | 0.703834 1.99837 1.22723 
439 | 0.709839 2.00507 1.22901 
440 | 0.692431 2.00348 1.22839 
441 | 0.701548 2.02345 1.2339 
442 | 0.708827 2.03802 1.23816 
443 | 0.718502 2.05234 1.2428 
444 | 0.727323 2.06604 1.24767 
445 | 0.736537 2.08045 1.25377 
446 | 0.760948 2.09755 1.26151 
447 | 0.767685 2.10203 1.26365 
448 | 0.772043 2.10487 1.26503 
449 | 0.75354 2.10058 1.26347 
450 | 0.783717 2.11813 1.27312 
451 | 0.791257 2.12222 1.27556 
452 | 0.799658 2.12665 1.27827 
453 | 0.807152 2.13048 1.28069 
454 | 0.815698 2.13471 1.28344 
455 | 0.774245 2.12058 1.27636 
456 | 0.806963 2.13375 1.28676 
457 | 0.81331 2.13607 1.28879 
458 | 0.818781 2.13802 1.29054 
459 | 0.824487 2.14 1.29237 
460 | 0.837856 2.1444 1.29662 
461 | 0.843948 2.1463 1.29855 
462 | 0.849527 2.14799 1.30031 
463 | 0.785785 2.12829 1.28687 
464 | 0.782961 2.12486 1.2882 
465 | 0.785939 2.12533 1.29244 
466 | 0.793936 2.12686 1.29682 
467 | 0.805814 2.12945 1.30248 
468 | 0.818796 2.13194 1.3092 
469 | 0.835346 2.13494 1.31773 
470 | 0.855608 2.13695 1.32808 
471 | 0.874987 2.13809 1.33946 
472 | 0.903238 2.13544 1.34938 
473 | 0.908265 2.13492 1.35116 
474 | 0.9073 2.13901 1.35625 
475 | 0.925889 2.13956 1.36897 
476 | 0.947079 2.14019 1.38188 
477 | 0.962491 2.14111 1.39267 
478 | 0.978426 2.14182 1.40297 
479 | 1.00492 2.13837 1.41249 
480 | 1.00856 2.13789 1.41377 
481 | 1.00498 2.14194 1.41708 
482 | 1.03637 2.13766 1.42811 
483 | 1.0266 2.14284 1.42952 
484 | 1.04485 2.14283 1.44 
485 | 1.0657 2.14305 1.45064 
486 | 1.08692 2.14258 1.46123 
487 | 1.10627 2.14236 1.47063 
488 | 1.12414 2.14195 1.47938 
489 | 1.14529 2.1423 1.48731 
490 | 1.16601 2.14241 1.49481 
491 | 1.1852 2.14176 1.50187 
492 | 1.20886 2.14081 1.50956 
493 | 1.23354 2.14017 1.5163 
494 | 1.26385 2.13443 1.52296 
495 | 1.26996 2.13325 1.52423 
496 | 1.28022 2.13126 1.52632 
497 | 1.29274 2.12881 1.52878 
498 | 1.30129 2.12713 1.5304 
499 | 1.28709 2.13363 1.52936 
500 | 1.30785 2.13261 1.53363 
501 | 1.33447 2.13117 1.53753 
502 | 1.37281 2.12275 1.54263 
503 | 1.37831 2.12154 1.54331 
504 | 1.38293 2.12665 1.54367 
505 | 1.42006 2.12361 1.54785 
506 | 1.46252 2.11474 1.55182 
507 | 1.474 2.11236 1.5528 
508 | 1.47729 2.11911 1.55343 
509 | 1.52484 2.11486 1.55873 
510 | 1.56609 2.11732 1.56224 
511 | 1.61318 2.11029 1.5652 
512 | 1.63209 2.10757 1.5663 
513 | 1.63535 2.11342 1.56762 
514 | 1.68803 2.10149 1.57271 
515 | 1.70523 2.1952 1.60768 
516 | 1.72428 2.18084 1.60547 
517 | 1.76827 2.16977 1.60406 
518 | 1.81671 2.16089 1.60386 
519 | 1.86384 2.15501 1.60406 
520 | 1.90846 2.15567 1.6076 
521 | 1.92347 2.15593 1.60882 
522 | 1.94019 2.14924 1.60731 
523 | 1.99356 2.14599 1.6097 
524 | 2.03386 2.14331 1.61141 
525 | 2.07471 2.14195 1.61321 
526 | 2.1132 2.14098 1.61545 
527 | 2.14904 2.13812 1.61772 
528 | 2.18235 2.13373 1.62041 
529 | 2.21711 2.12773 1.62371 
530 | 2.25348 2.12435 1.62688 
531 | 2.28237 2.12189 1.62956 
532 | 2.31144 2.11841 1.63289 
533 | 2.34413 2.11278 1.63738 
534 | 2.37277 2.10688 1.64167 
535 | 2.40081 2.10057 1.64598 
536 | 2.42762 2.09333 1.65048 
537 | 2.45075 2.08512 1.65525 
538 | 2.47182 2.07585 1.66024 
539 | 2.49069 2.0672 1.66553 
540 | 2.51021 2.05758 1.67096 
541 | 2.53393 2.0448 1.67771 
542 | 2.55481 2.03166 1.6837 
543 | 2.57276 2.0185 1.68978 
544 | 2.58703 2.00582 1.69508 
545 | 2.60123 1.99348 1.69967 
546 | 2.61388 1.97957 1.70421 
547 | 2.63305 1.96418 1.70971 
548 | 2.63484 1.96269 1.71023 
549 | 2.62958 1.96141 1.70989 
550 | 2.63927 1.9461 1.71396 
551 | 2.65062 1.92879 1.71832 
552 | 2.66243 1.91121 1.72205 
553 | 2.67108 1.89494 1.72584 
554 | 2.68512 1.8765 1.73051 
555 | 2.67999 1.8779 1.72944 
556 | 2.68602 1.86392 1.73252 
557 | 2.69183 1.84965 1.73552 
558 | 2.70373 1.82751 1.74081 
559 | 2.7046 1.82579 1.74122 
560 | 2.6985 1.82813 1.74016 
561 | 2.69956 1.81194 1.74356 
562 | 2.70021 1.79846 1.74649 
563 | 2.7005 1.78088 1.75036 
564 | 2.70007 1.76632 1.75361 
565 | 2.69817 1.75147 1.75689 
566 | 2.69686 1.73622 1.76004 
567 | 2.69394 1.71822 1.76376 
568 | 2.69544 1.69291 1.76902 
569 | 2.6957 1.68634 1.77035 
570 | 2.68906 1.68389 1.77113 
571 | 2.68881 1.65203 1.7773 
572 | 2.68875 1.6498 1.77772 
573 | 2.68215 1.64823 1.77877 
574 | 2.67766 1.62776 1.78312 
575 | 2.67099 1.61076 1.78675 
576 | 2.66339 1.59473 1.79029 
577 | 2.65711 1.56816 1.79506 
578 | 2.65656 1.56599 1.79544 
579 | 2.6528 1.57426 1.79453 
580 | 2.6423 1.55867 1.79763 
581 | 2.63257 1.53266 1.80175 
582 | 2.6317 1.53046 1.80208 
583 | 2.62804 1.53795 1.80111 
584 | 2.6155 1.52451 1.80287 
585 | 2.60564 1.51236 1.80408 
586 | 2.59261 1.48859 1.80662 
587 | 2.58768 1.47985 1.80746 
588 | 2.58215 1.48248 1.80622 
589 | 2.56112 1.46042 1.80736 
590 | 2.54345 1.44384 1.80774 
591 | 2.52354 1.41551 1.80969 
592 | 2.51536 1.40412 1.81042 
593 | 2.51096 1.39807 1.8108 
594 | 2.50591 1.4067 1.80923 
595 | 2.48196 1.38312 1.80904 
596 | 2.46306 1.36869 1.80848 
597 | 2.44346 1.35234 1.80764 
598 | 2.42432 1.33712 1.80674 
599 | 2.40064 1.31924 1.80524 
600 | 2.38084 1.30639 1.80363 
601 | 2.35782 1.29197 1.80183 
602 | 2.33293 1.27324 1.79959 
603 | 2.31013 1.25561 1.79708 
604 | 2.28749 1.23865 1.79465 
605 | 2.25704 1.21121 1.79259 
606 | 2.25463 1.20903 1.79242 
607 | 2.24734 1.20243 1.79189 
608 | 2.25189 1.21117 1.79011 
609 | 2.23318 1.19819 1.78703 
610 | 2.2149 1.18655 1.78387 
611 | 2.1821 1.16005 1.78085 
612 | 2.17165 1.15162 1.77986 
613 | 2.18433 1.16443 1.77914 
614 | 2.16647 1.15355 1.77589 
615 | 2.15107 1.14487 1.77271 
616 | 2.13439 1.13541 1.76947 
617 | 2.11672 1.1265 1.76608 
618 | 2.10216 1.11894 1.76281 
619 | 2.08898 1.11336 1.75979 
620 | 2.07541 1.10673 1.75638 
621 | 2.06026 1.099 1.7526 
622 | 2.03657 1.08673 1.74689 
623 | 2.0148 1.07685 1.74144 
624 | 1.99165 1.06429 1.7352 
625 | 1.97096 1.05256 1.72884 
626 | 1.94548 1.03968 1.72183 
627 | 1.92624 1.02851 1.71511 
628 | 1.90681 1.01779 1.70829 
629 | 1.88958 1.00789 1.70193 
630 | 1.875 0.999351 1.6959 
631 | 1.86151 0.991424 1.6901 
632 | 1.83488 0.980064 1.68379 
633 | 1.84618 0.982613 1.68273 
634 | 1.81587 0.970489 1.67516 
635 | 1.82996 0.972709 1.67376 
636 | 1.81557 0.964907 1.66573 
637 | 1.7853 0.954598 1.65723 
638 | 1.78209 0.953524 1.65631 
639 | 1.77686 0.951774 1.65481 
640 | 1.76427 0.947588 1.65113 
641 | 1.8006 0.95556 1.65459 
642 | 1.79476 0.951944 1.64806 
643 | 1.76476 0.944738 1.63889 
644 | 1.78501 0.947858 1.64134 
645 | 1.77547 0.944541 1.63557 
646 | 1.76968 0.941589 1.63099 
647 | 1.74088 0.937655 1.62247 
648 | 1.73425 0.936809 1.62054 
649 | 1.72853 0.936095 1.61888 
650 | 1.73781 0.929764 1.6148 
651 | 1.70987 0.926717 1.60649 
652 | 1.7077 0.926499 1.60585 
653 | 1.70178 0.925918 1.60411 
654 | 1.69442 0.925222 1.60193 
655 | 1.71562 0.920461 1.60051 
656 | 1.68544 0.918665 1.59134 
657 | 1.68164 0.918488 1.5902 
658 | 1.70038 0.914426 1.5909 
659 | 1.69271 0.91033 1.58525 
660 | 1.66975 0.910379 1.57807 
661 | 1.68178 0.906521 1.57836 
662 | 1.652 0.907434 1.56882 
663 | 1.64689 0.907657 1.56717 
664 | 1.66324 0.902949 1.56872 
665 | 1.65164 0.90043 1.56234 
666 | 1.64025 0.897938 1.55701 
667 | 1.61469 0.893155 1.54693 
668 | 1.59447 0.888317 1.53712 
669 | 1.57318 0.883605 1.52685 
670 | 1.55357 0.87889 1.51745 
671 | 1.54007 0.874509 1.51001 
672 | 1.51819 0.877566 1.5032 
673 | 1.5112 0.878573 1.50107 
674 | 1.51391 0.870354 1.49795 
675 | 1.49683 0.866432 1.48896 
676 | 1.48274 0.862863 1.48159 
677 | 1.46825 0.859704 1.47458 
678 | 1.45046 0.856345 1.46728 
679 | 1.43282 0.853635 1.46072 
680 | 1.41685 0.851338 1.45459 
681 | 1.39662 0.849351 1.4476 
682 | 1.37585 0.847258 1.44143 
683 | 1.35404 0.845441 1.43484 
684 | 1.33263 0.849959 1.42986 
685 | 1.32884 0.843511 1.42812 
686 | 1.30471 0.841631 1.42256 
687 | 1.27954 0.840702 1.41689 
688 | 1.25082 0.841315 1.41082 
689 | 1.22315 0.847959 1.40524 
690 | 1.21476 0.850075 1.40358 
691 | 1.20465 0.844869 1.40142 
692 | 1.17135 0.846938 1.39467 
693 | 1.14511 0.84819 1.3895 
694 | 1.11898 0.849959 1.38458 
695 | 1.09295 0.852532 1.37983 
696 | 1.06257 0.861968 1.37446 
697 | 1.06623 0.856031 1.3747 
698 | 1.04087 0.859702 1.36985 
699 | 1.01054 0.864884 1.36439 
700 | 0.98804 0.868852 1.35998 
701 | 0.964944 0.873061 1.35575 
702 | 0.940318 0.881917 1.35168 
703 | 0.937798 0.882838 1.35126 
704 | 0.929167 0.880171 1.34937 
705 | 0.903578 0.885989 1.34415 
706 | 0.876966 0.895572 1.33984 
707 | 0.868705 0.891943 1.33802 
708 | 0.835173 0.898842 1.33208 
709 | 0.801184 0.90463 1.32651 
710 | 0.767421 0.910279 1.3213 
711 | 0.736737 0.915274 1.31648 
712 | 0.70729 0.921595 1.31178 
713 | 0.680048 0.927458 1.30761 
714 | 0.6532 0.933207 1.30382 
715 | 0.625872 0.939219 1.30008 
716 | 0.599797 0.944952 1.29654 
717 | 0.57239 0.949061 1.29334 
718 | 0.548579 0.952398 1.29036 
719 | 0.52296 0.956472 1.28704 
720 | 0.48971 0.963389 1.28288 
721 | 0.457212 0.968724 1.27901 
722 | 0.428365 0.972417 1.2758 
723 | 0.400819 0.975503 1.27284 
724 | 0.375697 0.978741 1.26992 
725 | 0.350577 0.98184 1.26724 
726 | 0.325455 0.985056 1.26447 
727 | 0.303027 0.987855 1.26203 
728 | 0.279718 0.990556 1.25979 
729 | 0.257788 0.993552 1.2575 
730 | 0.227248 0.995785 1.25489 
731 | 0.209373 1.00161 1.2531 
732 | 0.203887 0.997106 1.25235 
733 | 0.185709 1.00304 1.25048 
734 | 0.177696 0.999049 1.24955 
735 | 0.159727 1.00494 1.24763 
736 | 0.157586 1.00565 1.2474 
737 | 0.149962 1.00398 1.24611 
738 | 0.12675 1.00833 1.24292 
739 | 0.108841 1.01127 1.24055 
740 | 0.0893653 1.01509 1.23823 
741 | 0.0659569 1.01852 1.23606 
742 | 0.0474836 1.02212 1.23387 
743 | 0.0309857 1.02456 1.23202 
744 | 0.01396 1.02731 1.23009 
745 | 0.00184054 1.03031 1.22799 
746 | -0.0119215 1.03405 1.22564 
747 | -0.022565 1.0405 1.22367 
748 | -0.0251121 1.04216 1.22316 
749 | -0.0226702 1.04428 1.22164 
750 | -0.0335612 1.05275 1.21887 
751 | -0.0366573 1.05549 1.21796 
752 | -0.0548841 1.05776 1.21473 
753 | -0.0738157 1.06786 1.20903 
754 | -0.0790276 1.07443 1.20653 
755 | -0.0811536 1.07752 1.20534 
756 | -0.0668597 1.07755 1.20566 
757 | -0.0714219 1.08489 1.20274 
758 | -0.0737141 1.09271 1.19954 
759 | -0.0715174 1.09222 1.1997 
760 | -0.0726953 1.10039 1.1963 
761 | -0.072839 1.10246 1.19543 
762 | -0.0729514 1.10645 1.19375 
763 | 


--------------------------------------------------------------------------------
/CPPFiles/fmUKF.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Unscented Kalman Filter
  3 |  * State: Position, Velocity, Orientation, Accelerometer Bias, Roll/Pitch Bias
  4 |  * Fredy Monterroza 
  5 |  * 
  6 | */
  7 | 
  8 | #include <iostream>
  9 | //#include <fstream>
 10 | #include <stdio.h>
 11 | #include <stdlib.h>
 12 | #include <string.h>
 13 | 
 14 | #include <math.h>
 15 | 
 16 | #include <boost/numeric/ublas/vector.hpp>
 17 | #include <boost/numeric/ublas/matrix.hpp>
 18 | #include <boost/numeric/ublas/io.hpp>
 19 | 
 20 | 
 21 | #include "cholesky.hpp"
 22 | #include "InvertMatrix.hpp"
 23 | 
 24 | using std::cout;
 25 | using std::cin;
 26 | using std::endl;
 27 | 
 28 | using namespace boost::numeric;
 29 | 
 30 | ublas::vector<double> gEqn(ublas::vector<double>, ublas::vector<double>, ublas::vector<double>, double);
 31 | ublas::matrix<double> RPYtoRotMat(double, double, double); //Roll, Pitch, Yaw
 32 | double atanFM(double, double);
 33 | ublas::matrix<double> calculateCovEquation(ublas::matrix<double>, ublas::matrix<double>);
 34 | 
 35 | ublas::vector<double> sigmaPoint(14);
 36 | extern ublas::matrix<double> Q;
 37 | extern ublas::matrix<double> R;
 38 | extern ublas::vector<double> Xest;
 39 | extern int kfIteration;
 40 | 
 41 | /*
 42 |  * Using the Unscented Kalman Filter, compute a state estimate from the gEqn() quadrotor dynamics
 43 |  * and update this apriori state estimate whenever a measurement is available from the nPointPose
 44 |  * algorithm. 
 45 |  * @param stateVector Container to hold the computed state estimates.
 46 |  * @param x Previous State Estimate
 47 |  * @param P Previous State Covariance
 48 |  * @param deltaT Sample Rate, time at which data collected on quadrotor
 49 |  * @param z Measurement provided by N-Point Pose Algorithm 
 50 |  * @param errVn Disturbance used in tracking accelerometer bias
 51 |  * @param Vn Control input from IMU. 
 52 |  */
 53 | void fmUKF (std::vector<dataVec>& stateVector, ublas::vector<double> x, ublas::matrix<double>& P, double deltaT, ublas::vector<double> z, ublas::vector<double> errVn, ublas::vector<double> Vn)
 54 | 
 55 | {
 56 | 	//cout << "fmUKF" << endl;
 57 | 
 58 | 	ublas::matrix<double> sumPQ = P + Q;
 59 | 	ublas::matrix<double> L(14, 14);
 60 | 	size_t decompFail = cholesky_decompose(sumPQ, L);
 61 | 	ublas::matrix<double> W_i(14,28);
 62 | 	ublas::vector<double> aprioriStateEst(14);
 63 | 	std::fill(aprioriStateEst.begin(), aprioriStateEst.end(), 0.0);
 64 | 	ublas::matrix<double> X_i(14,28);
 65 | 
 66 | 	if (decompFail == 0) {
 67 | 		//Capture the Covariance
 68 | 		subrange(W_i, 0, 14, 0, 14) = sqrt(14.0) * L;
 69 | 		subrange(W_i, 0, 14, 14, 28) = -sqrt(14.0) * L;
 70 | 
 71 | 		//Add the previous mean Xest to complete Sigma Points
 72 | 		for(unsigned i = 0; i < W_i.size2(); ++i){ //bsxfun(@plus)
 73 | 			column(W_i, i) = column(W_i, i) + x;
 74 | 		}
 75 | 		//cout << W_i << endl;
 76 | 
 77 | 
 78 | 	} else { 
 79 | 		cout << "Require Square Symmertric Positive Definite Input" << endl;
 80 | 		//Reset L to identity. 
 81 | 		for(ublas::matrix<double>::iterator1 iter1L = L.begin1(); iter1L != L.end1(); ++iter1L){ //Row by row
 82 | 			for(ublas::matrix<double>::iterator2 iter2L = iter1L.begin(); iter2L != iter1L.end(); ++iter2L){ //Go from col to the next col for fixed row
 83 | 				if (iter1L.index1() == iter2L.index2()){
 84 | 					*iter2L = 0.1225; //Cholesky of Initial P + Q //1;
 85 | 				} else {
 86 | 					*iter2L = 0;
 87 | 				}
 88 | 			}
 89 | 	    }
 90 | 		subrange(W_i, 0, 14, 0, 14) = sqrt(14.0) * L;
 91 | 		subrange(W_i, 0, 14, 14, 28) = -sqrt(14.0) * L;
 92 | 
 93 | 		//Add the previous mean Xest to complete Sigma Points
 94 | 		for(unsigned i = 0; i < W_i.size2(); ++i){ //bsxfun(@plus)
 95 | 			column(W_i, i) = column(W_i, i) + x;
 96 | 		}
 97 | 	}
 98 | 	//cout << W_i << endl;
 99 | 
100 | 	//Propogate the Sigma Points
101 | 	ublas::vector<double> propogatedSigma(14);
102 | 	for(unsigned i = 0; i < W_i.size2(); ++i){
103 | 		sigmaPoint = column(W_i, i);
104 | 		propogatedSigma = gEqn(sigmaPoint, errVn, Vn, deltaT);
105 | 		//cout << kfIteration << endl;
106 | 		//cout << propogatedSigma << endl;
107 | 	    column(X_i, i) = propogatedSigma;
108 | 	    aprioriStateEst += propogatedSigma;
109 | 	}
110 | 	
111 | 	aprioriStateEst /= X_i.size2(); //Mean
112 | 
113 | 	//W_i is W_i_prime (Propogated Sigma Points - Mean subtracted)
114 | 	for(unsigned i = 0; i < X_i.size2(); ++i){
115 | 		column(W_i, i) = column(X_i, i) - aprioriStateEst;
116 | 	}
117 | 
118 | 	ublas::matrix<double> Pbar_k = calculateCovEquation(W_i, W_i);
119 | 
120 | 	//No Measurement Update Available
121 | 	if (z(0)+z(1)+z(2)+z(3)+z(4)+z(5) == 0){
122 | 		P = Pbar_k; 
123 | 		dataVec tempVec;
124 | 		for(unsigned i = 0; i < aprioriStateEst.size(); ++i){
125 | 			tempVec.push_back(aprioriStateEst(i));
126 | 			Xest(i) = aprioriStateEst(i);
127 | 		}
128 | 		stateVector.push_back(tempVec);
129 | 		//cout << "No Measurement Update Available to Correct Apriori Estimate" << endl;
130 | 		return;
131 | 	}
132 | 
133 | 	//Predicted Observation Zbar_k, Observation subset Z_i
134 | 	ublas::matrix<double> Z_i(6, 28); //Pos, Orient
135 | 	ublas::vector<double> Zbar_k(6);
136 | 	Zbar_k(0) = 0; Zbar_k(1) = 0; Zbar_k(2) = 0; Zbar_k(3) = 0; Zbar_k(4) = 0; Zbar_k(5) = 0;
137 | 	for(unsigned i = 0; i < X_i.size2(); ++i){
138 | 		Z_i(0, i) = X_i(0, i); Z_i(1, i) = X_i(1, i); Z_i(2, i) = X_i(2, i);
139 | 		Z_i(3, i) = X_i(6, i); Z_i(4, i) = X_i(7, i); Z_i(5, i) = X_i(8, i);
140 | 		Zbar_k += column(Z_i, i);
141 | 	}
142 | 	//Note: These will vary from matlab since this involves propogated sigma points, random walk model.
143 | 	Zbar_k /= 28;
144 | 
145 | 	//Innovation Vector (Residual between Actual and Expected Measurement
146 | 	ublas::vector<double> Vk = z - Zbar_k;
147 | 
148 | 	//Recycle, Reuse, Reduce: Observation subset with mean subtracted.
149 | 	for(unsigned i = 0; i < Z_i.size2(); ++i){
150 | 		column(Z_i, i) = column(Z_i, i) - Zbar_k;
151 | 	}
152 | 
153 | 
154 | 	//Innovation Covariance
155 | 	ublas::matrix<double> Pvv = calculateCovEquation(Z_i, Z_i);
156 | 	Pvv += R;
157 | 
158 | 	//Cross Correlation
159 | 	ublas::matrix<double> Pxz = calculateCovEquation(W_i, Z_i);
160 | 
161 | 	ublas::matrix<double> PvvInv(Pvv.size1(), Pvv.size2());
162 | 	if(InvertMatrix(Pvv, PvvInv)){ //via LU Factorization
163 | 		
164 | 		//Kalman Gain
165 | 		ublas::matrix<double> K_k = prod(Pxz, PvvInv);
166 | 		
167 | 		//Measurement Updated State Estimate
168 | 		ublas::vector<double> tempProd = prod(K_k, Vk); //These placeholders are necessary to store result of prod()
169 | 		ublas::vector<double> aposterioriStateEst = aprioriStateEst + tempProd;
170 | 		
171 | 		dataVec tempVec;
172 | 		for(unsigned i = 0; i < aposterioriStateEst.size(); ++i){
173 | 			tempVec.push_back(aposterioriStateEst(i));
174 | 			Xest(i) = aposterioriStateEst(i);
175 | 		}
176 | 		stateVector.push_back(tempVec);
177 | 
178 | 		ublas::matrix<double> holdProd = prod(Pvv, trans(K_k)); 
179 | 		ublas::matrix<double> tempProd2 = prod(K_k, holdProd);
180 | 		P = Pbar_k - tempProd2;//prod(K_k, holdProd);
181 | 
182 | 	}else {cout << "Pvv Approaching Singularity" << endl;}
183 | 
184 | 
185 | 
186 | }
187 | 
188 | /*
189 |  * Quadrotor (Non-Linear) Dynamics
190 |  * @param sigmaPoint Sigma Point captured by Cholesky Decomposition and Previous State Estimate
191 |  * @param errVn Disturbance used to track accelerometer bias/
192 |  * @param Vn Control Input from IMU
193 |  * @param deltaT Difference in time between sample collection on quadrotor.
194 |  * @return propogated sigma point.
195 |  */
196 | ublas::vector<double> gEqn(ublas::vector<double> sigmaPoint, ublas::vector<double> errVn, ublas::vector<double> Vn, double deltaT){	
197 | 	
198 | 	ublas::vector<double> propState(14);
199 | 
200 | 	ublas::matrix<double> rotMat = RPYtoRotMat(sigmaPoint(6), sigmaPoint(7), sigmaPoint(8));
201 | 
202 | 	//Acceleration
203 | 	ublas::vector<double> tempAccInput(3); tempAccInput(0) = Vn(0) - sigmaPoint(9); tempAccInput(1) = Vn(1) - sigmaPoint(10); tempAccInput(2) = Vn(2) - sigmaPoint(11); //IMU Accel - Tracked Bias
204 | 	ublas::vector<double> worldAcc = prod(rotMat, tempAccInput);
205 | 	worldAcc(0) = worldAcc(0) - .4109; worldAcc(1) = worldAcc(1) - .4024; worldAcc(2) = worldAcc(2) - 9.6343; //Subtract Gravity 
206 | 
207 | 	//Position
208 | 	propState(0) = sigmaPoint(0) + (sigmaPoint(3)*deltaT) + (0.5*worldAcc(0)*deltaT*deltaT);
209 | 	propState(1) = sigmaPoint(1) + (sigmaPoint(4)*deltaT) + (0.5*worldAcc(1)*deltaT*deltaT);
210 | 	propState(2) = sigmaPoint(2) + (sigmaPoint(5)*deltaT) + (0.5*worldAcc(2)*deltaT*deltaT);
211 | 
212 | 	//Velocity
213 | 	propState(3) = sigmaPoint(3) + (worldAcc(0)*deltaT);
214 | 	propState(4) = sigmaPoint(4) + (worldAcc(1)*deltaT);
215 | 	propState(5) = sigmaPoint(5) + (worldAcc(2)*deltaT);
216 | 
217 | 	//Orientation: Radians
218 | 	ublas::matrix<double> gyroIntegration = RPYtoRotMat(Vn(3)*deltaT, Vn(4)*deltaT, Vn(5)*deltaT);
219 | 	ublas::matrix<double> tempQuadOrientMat = prod(rotMat, gyroIntegration);
220 | 
221 | 	propState(6) = asin(tempQuadOrientMat(1,2));
222 | 	propState(7) = atanFM(-tempQuadOrientMat(0,2)/cos(propState(6)),tempQuadOrientMat(2,2)/cos(propState(6)));
223 | 	propState(8) = atanFM(-tempQuadOrientMat(1,0)/cos(propState(6)),tempQuadOrientMat(1,1)/cos(propState(6)));
224 | 
225 | 
226 | 	//Accelerometer Bias (Random Walk)
227 | 	propState(9) = sigmaPoint(9) + (errVn(6)*deltaT); propState(10) = sigmaPoint(10) + (errVn(7)*deltaT); propState(11) = sigmaPoint(11) + (errVn(8)*deltaT);
228 | 
229 | 	propState(12) = sigmaPoint(12); propState(13) = sigmaPoint(13);
230 | 
231 | 	return propState;
232 | 
233 | }
234 | 
235 | /*
236 |  * Return Rotation Matrix along ZXY
237 |  * @param phi Roll angle in radians
238 |  * @param theta Pitch angle in radians
239 |  * @param psi Yaw angle in radians
240 |  * @param Orientation of Quadrotor in world frame
241 |  */
242 | ublas::matrix<double> RPYtoRotMat(double phi, double theta, double psi){
243 | 	ublas::matrix<double> rotMat(3,3);
244 | 	rotMat(0,0) = cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta);
245 | 	rotMat(0,1) = cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta);
246 | 	rotMat(0,2) = -cos(phi)*sin(theta);
247 | 
248 | 	rotMat(1,0) =  -cos(phi)*sin(psi);
249 | 	rotMat(1,1) = cos(phi)*cos(psi); 
250 | 	rotMat(1,2) = sin(phi);
251 | 
252 | 	rotMat(2,0) = cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi);
253 | 	rotMat(2,1) = sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi);
254 | 	rotMat(2,2) = cos(phi)*cos(theta);
255 | 
256 | 	return rotMat;
257 | }
258 | 
259 | /*
260 |  * atan used with Matlab Symbolic Explressions, replicated here for comparison.
261 |  * @return Inverse Tangent in radians.
262 |  */
263 | double atanFM(double y, double x){
264 | 	return  2*atan(y / ( sqrt((x*x)+(y*y)) + x ));	
265 | }
266 | 
267 | /*
268 |  * Covariance Matrix calculated as weighted/averaged outer product of input points/vectors.
269 |  * @return Covariance Matrix obtained between A, B vectors.
270 |  */
271 | ublas::matrix<double> calculateCovEquation(ublas::matrix<double> A, ublas::matrix<double> B){
272 | 
273 | 	//ublas::matrix<double> covMat(14, 14);
274 | 	ublas::matrix<double> covMat(A.size1(), B.size1());
275 | 	for(unsigned i = 0; i < covMat.size1(); ++i){
276 | 		for(unsigned j = 0; j < covMat.size2(); ++j){ 
277 | 			covMat(i, j) = 0.0;
278 | 		}
279 | 	}
280 | 
281 | 	//ublas::vector<double> transposeVec;
282 | 	for(unsigned i = 0; i < A.size2(); ++i){
283 | 		covMat += outer_prod(column(A, i), column(B, i));//outer_prod(colA, colB);//outer_prod(column(A, i), column(B, i));
284 | 	}
285 | 	covMat /= 28;
286 | 
287 | 	return covMat;
288 | }
289 | 
290 | 
291 | 
292 | 
293 | 
294 |    
295 |       
296 | 


--------------------------------------------------------------------------------
/CPPFiles/inputData.cpp:
--------------------------------------------------------------------------------
  1 | /* 
  2 |    This file is to read in the quadData.mat cell structure containing quadLog, quadTime, and sensorLog and parse 
  3 |    it into corresponding data structures necessary for implementation of the EKF/UKF.
  4 |    Fredy Monterroza
  5 | */
  6 | 
  7 | #include <iostream>
  8 | #include <fstream>
  9 | #include <stdio.h>
 10 | #include <stdlib.h>
 11 | #include <string.h>
 12 | #include <iomanip>
 13 | #include <sstream>
 14 | 
 15 | #include <algorithm>
 16 | #include <vector>
 17 | #include <map>
 18 | #include <list>
 19 | 
 20 | 
 21 | 
 22 | //using namespace std;
 23 | using std::cout;
 24 | using std::cin;
 25 | using std::endl;
 26 | using std::vector;
 27 | using std::string;
 28 | using std::stringstream;
 29 | using std::list;
 30 | using std::ifstream;
 31 | 
 32 | 
 33 | typedef vector<double> dataVec;
 34 | 
 35 | dataVec parseStringForData(string&);
 36 | 
 37 | std::vector<dataVec> sensorLogAccImu;
 38 | std::vector<dataVec> sensorLogOmegaImu; 
 39 | std::vector<double> quadTimeLog;
 40 | std::vector<dataVec> visionRobotPos; 
 41 | std::vector<dataVec> visionRobotOrient; 
 42 | std::vector<dataVec> viconEuler; 
 43 | std::vector<dataVec> viconPos;
 44 | std::list<double> viconTime;
 45 | 
 46 | //void inputData(void);
 47 | //int main() { //void inputData()
 48 | void inputData(){
 49 | 
 50 |  //-----Read Process Input (Control Input, IMU Accelerometer)-----
 51 |  //vector<dataVec> sensorLogAccImu; 
 52 |  string inSensorAccel;
 53 |  ifstream inFileSensorAccel ("../Data/processInputAcc.txt");
 54 |  if(inFileSensorAccel.is_open()){
 55 |   while(getline(inFileSensorAccel, inSensorAccel)){
 56 |    dataVec imuAccel;
 57 |    imuAccel = parseStringForData(inSensorAccel);
 58 |    sensorLogAccImu.push_back(imuAccel);
 59 |   }
 60 |   inFileSensorAccel.close();
 61 |  }else{
 62 |   cout << "Error Opening File" << endl;
 63 |  }
 64 | 
 65 |  /* 
 66 |  for(vector<dataVec>::iterator imuAccelIter = sensorLogAccImu.begin(); imuAccelIter != sensorLogAccImu.end(); ++imuAccelIter){
 67 |   for(dataVec::iterator accelImuIter = imuAccelIter->begin(); accelImuIter != imuAccelIter->end(); ++accelImuIter){
 68 |    cout << *accelImuIter << " ";
 69 |   }
 70 |   cout << endl;
 71 |  }
 72 |  */
 73 |  
 74 |  //-----Read Process Input (Control Input, IMU Gyroscope, Angular Velocity)-----
 75 |  //vector<dataVec> sensorLogOmegaImu; 
 76 |  string inSensorOmega;
 77 |  ifstream inFileSensorOmega ("../Data/processInputOmega.txt");
 78 |  if(inFileSensorOmega.is_open()){
 79 |   while(getline(inFileSensorOmega, inSensorOmega)){
 80 |    dataVec imuOmega;
 81 |    imuOmega = parseStringForData(inSensorOmega);
 82 |    sensorLogOmegaImu.push_back(imuOmega);
 83 |   }
 84 |   inFileSensorOmega.close();
 85 |  }else{
 86 |   cout << "Error Opening File" << endl;
 87 |  }
 88 |  /*
 89 |  for(vector<dataVec>::iterator imuOmegaIter = sensorLogOmegaImu.begin(); imuOmegaIter != sensorLogOmegaImu.end(); ++imuOmegaIter){
 90 |   for(dataVec::iterator omegaImuIter = imuOmegaIter->begin(); omegaImuIter != imuOmegaIter->end(); ++omegaImuIter){
 91 |    cout << *omegaImuIter << " ";
 92 |   }
 93 |   cout << endl;
 94 |  }
 95 |  */
 96 |  
 97 |   //-----Read Quad Sensor Times------
 98 |  string inQuadTime;
 99 |  double inQuadTimeNum;
100 |  ifstream inFileQuadTime ("../Data/quadTimeLog.txt");
101 |  if(inFileQuadTime.is_open()){
102 |   while(getline(inFileQuadTime, inQuadTime)){
103 |    inQuadTimeNum = stod(inQuadTime);
104 |    quadTimeLog.push_back(inQuadTimeNum);
105 |   }
106 |   inFileQuadTime.close();
107 |  }else {
108 |  cout << "Error Opening File" << endl;
109 |  }
110 |  
111 |  
112 |  //-----Read poseNPPPos (Measurement Update, Position Output From nPointPose Algorithm)-----
113 |  //vector<dataVec> visionRobotPos; 
114 |  string inVisionRobotPos;
115 |  ifstream inFileVisionRobotPos ("../Data/poseNPPPos.txt");
116 |  if(inFileVisionRobotPos.is_open()){
117 |   while(getline(inFileVisionRobotPos, inVisionRobotPos)){
118 |    dataVec robotPos;
119 |    robotPos = parseStringForData(inVisionRobotPos);
120 |    visionRobotPos.push_back(robotPos);
121 |   }
122 |   inFileVisionRobotPos.close();
123 |  }else{
124 |   cout << "Error Opening File" << endl;
125 |  }
126 |  /*
127 |  for(vector<dataVec>::iterator visionPosIter = visionRobotPos.begin(); visionPosIter != visionRobotPos.end(); ++visionPosIter){
128 |   for(dataVec::iterator posVisionIter = visionPosIter->begin(); posVisionIter != visionPosIter->end(); ++posVisionIter){
129 |    cout << *posVisionIter << " ";
130 |   }
131 |   cout << endl;
132 |  }
133 | */
134 | 
135 |  //-----Read poseNPPOrient (Measurement Update, Orientation Output From nPointPose Algorithm)-----
136 |  //vector<dataVec> visionRobotOrient; 
137 |  string inVisionRobotOrient;
138 |  ifstream inFileVisionRobotOrient ("../Data/poseNPPOrient.txt");
139 |  if(inFileVisionRobotOrient.is_open()){
140 |   while(getline(inFileVisionRobotOrient, inVisionRobotOrient)){
141 |    dataVec robotOrient;
142 |    robotOrient = parseStringForData(inVisionRobotOrient);
143 |    visionRobotOrient.push_back(robotOrient);
144 |   }
145 |   inFileVisionRobotOrient.close();
146 |  }else{
147 |   cout << "Error Opening File" << endl;
148 |  }
149 |  /*
150 |  for(vector<dataVec>::iterator visionOrientIter = visionRobotOrient.begin(); visionOrientIter != visionRobotOrient.end(); ++visionOrientIter){
151 |   for(dataVec::iterator orientVisionIter = visionOrientIter->begin(); orientVisionIter != visionOrientIter->end(); ++orientVisionIter){
152 |    cout << *orientVisionIter << " ";
153 |   }
154 |   cout << endl;
155 |  }
156 | */
157 | 
158 | 
159 | 
160 |  //-----Read Vicon Euler Angles-----
161 |  //vector<dataVec> viconEuler; 
162 |  string inEuler;
163 |  //ifstream inFileEuler ("quadData/viconEuler.txt");
164 |  ifstream inFileEuler ("../Data/viconEuler.txt");
165 |  if(inFileEuler.is_open()){
166 |   while(getline(inFileEuler, inEuler)){
167 |    dataVec euler;
168 |    euler = parseStringForData(inEuler);
169 |    viconEuler.push_back(euler);
170 |   }
171 |   inFileEuler.close();
172 |  }else{
173 |   cout << "Error Opening File" << endl;
174 |  }
175 |  /*
176 |  for(vector<dataVec>::iterator eulerIter = viconEuler.begin(); eulerIter != viconEuler.end(); ++eulerIter){
177 |   for(dataVec::iterator iterAngs = eulerIter->begin(); iterAngs != eulerIter->end(); ++iterAngs){
178 |    cout << *iterAngs << " ";
179 |   }
180 |   cout << endl;
181 |  }
182 |  */
183 | 
184 |  //-----Read Vicon Position-----
185 |  //vector<dataVec> viconPos; 
186 |  string inPos;
187 |  //ifstream inFilePos ("quadData/viconPos.txt");
188 |  ifstream inFilePos ("../Data/viconPos.txt");
189 |  if(inFilePos.is_open()){
190 |   while(getline(inFilePos, inPos)){
191 |    dataVec pos;
192 |    pos = parseStringForData(inPos);
193 |    viconPos.push_back(pos);
194 |   }
195 |   inFilePos.close();
196 |  }else{
197 |   cout << "Error Opening File" << endl;
198 |  }
199 |  
200 |  /*
201 |  for(vector<dataVec>::iterator posIter = viconPos.begin(); posIter != viconPos.end(); ++posIter){
202 |   for(dataVec::iterator iterPos = posIter->begin(); iterPos != posIter->end(); ++iterPos){
203 |    cout << *iterPos << " ";
204 |   }
205 |   cout << endl;
206 |  }
207 |  */
208 | 
209 | 
210 |  //-----Read Vicon Times------
211 |  //list<double> viconTime;
212 |  string inTime;
213 |  double inTimeNum;
214 |  //ifstream inFileTime ("quadData/viconTime.txt");
215 |  ifstream inFileTime ("../Data/viconTime.txt");
216 |  if(inFileTime.is_open()){
217 |   while(getline(inFileTime, inTime)){
218 |    //inTimeNum = atof(inTime.c_str());
219 |    //printf("%f\n", inTimeNum);
220 |    inTimeNum = stod(inTime);
221 |    //cout << setprecision(17) << inTimeNum << endl;
222 |    viconTime.push_back(inTimeNum);
223 |  
224 |   }
225 |   inFileTime.close();
226 |  }else {
227 |  cout << "Error Opening File" << endl;
228 |  }
229 |  /*
230 |  for (list<double>::iterator vtIter = viconTime.begin(); vtIter != viconTime.end(); ++vtIter){
231 |   cout << setprecision(17) << *vtIter << endl;
232 |  }
233 |  */
234 |  
235 | }
236 | 
237 | dataVec parseStringForData(string& inData){
238 |  string dataElement;
239 |  stringstream dataStream(inData);
240 |  dataVec dataVector;
241 |  double dataElementNum;
242 |  while(dataStream >> dataElement){
243 |   dataElementNum = stod(dataElement);
244 |   //cout << setprecision(17) << dataElementNum << endl;
245 |   dataVector.push_back(dataElementNum);
246 |  }
247 | 
248 |  return dataVector;
249 | 
250 | }
251 | 
252 | /*
253 | 
254 |  //-----Read Sensor (IMU) Accelerometer-----
255 |  vector<dataVec> sensorLogAccImu; 
256 |  string inSensorAccel;
257 |  ifstream inFileSensorAccel ("../Data/sensorIMUaccel.txt");
258 |  if(inFileSensorAccel.is_open()){
259 |   while(getline(inFileSensorAccel, inSensorAccel)){
260 |    dataVec imuAccel;
261 |    imuAccel = parseStringForData(inSensorAccel);
262 |    sensorLogAccImu.push_back(imuAccel);
263 |   }
264 |   inFileSensorAccel.close();
265 |  }else{
266 |   cout << "Error Opening File" << endl;
267 |  }
268 |  
269 |  for(vector<dataVec>::iterator imuAccelIter = sensorLogAccImu.begin(); imuAccelIter != sensorLogAccImu.end(); ++imuAccelIter){
270 |   for(dataVec::iterator accelImuIter = imuAccelIter->begin(); accelImuIter != imuAccelIter->end(); ++accelImuIter){
271 |    cout << *accelImuIter << " ";
272 |   }
273 |   cout << endl;
274 |  }
275 |  
276 | 
277 |  //-----Read Sensor (IMU) Gyroscope (Angular Velocity)-----
278 |  vector<dataVec> sensorLogOmegaImu; 
279 |  string inSensorOmega;
280 |  ifstream inFileSensorOmega ("../Data/sensorIMUangVel.txt");
281 |  if(inFileSensorOmega.is_open()){
282 |   while(getline(inFileSensorOmega, inSensorOmega)){
283 |    dataVec imuOmega;
284 |    imuOmega = parseStringForData(inSensorOmega);
285 |    sensorLogOmegaImu.push_back(imuOmega);
286 |   }
287 |   inFileSensorOmega.close();
288 |  }else{
289 |   cout << "Error Opening File" << endl;
290 |  }
291 |  
292 |  for(vector<dataVec>::iterator imuOmegaIter = sensorLogOmegaImu.begin(); imuOmegaIter != sensorLogOmegaImu.end(); ++imuOmegaIter){
293 |   for(dataVec::iterator omegaImuIter = imuOmegaIter->begin(); omegaImuIter != imuOmegaIter->end(); ++omegaImuIter){
294 |    cout << *omegaImuIter << " ";
295 |   }
296 |   cout << endl;
297 |  }
298 | 
299 |  
300 |   //-----Read visionRobotPos (Position Output From nPointPose Algorithm)-----
301 |  vector<dataVec> visionRobotPos; 
302 |  string inVisionRobotPos;
303 |  ifstream inFileVisionRobotPos ("../Data/visionRobotPos.txt");
304 |  if(inFileVisionRobotPos.is_open()){
305 |   while(getline(inFileVisionRobotPos, inVisionRobotPos)){
306 |    dataVec robotPos;
307 |    robotPos = parseStringForData(inVisionRobotPos);
308 |    visionRobotPos.push_back(robotPos);
309 |   }
310 |   inFileVisionRobotPos.close();
311 |  }else{
312 |   cout << "Error Opening File" << endl;
313 |  }
314 |  
315 |  for(vector<dataVec>::iterator visionPosIter = visionRobotPos.begin(); visionPosIter != visionRobotPos.end(); ++visionPosIter){
316 |   for(dataVec::iterator posVisionIter = visionPosIter->begin(); posVisionIter != visionPosIter->end(); ++posVisionIter){
317 |    cout << *posVisionIter << " ";
318 |   }
319 |   cout << endl;
320 |  }
321 |  
322 | */
323 | 
324 | 


--------------------------------------------------------------------------------
/CPPFiles/outputDataUKF.cpp:
--------------------------------------------------------------------------------
 1 | /*
 2 |  * Write the resulting state estimates contained in stateVector to a text file
 3 |  * 
 4 |  */
 5 | 
 6 | #include <iostream>
 7 | #include <fstream>
 8 | #include <boost/numeric/ublas/vector.hpp>
 9 | #include <boost/numeric/ublas/matrix.hpp>
10 | #include <boost/numeric/ublas/io.hpp>
11 | 
12 | 
13 | void outputDataUKF(std::vector<dataVec>& stateVector) {
14 |   std::ofstream outFile ("cppUKFData.txt");
15 |   short position;
16 |   if (outFile.is_open())
17 |   {
18 |     for(vector<dataVec>::iterator stateVectorIter = stateVector.begin(); stateVectorIter != stateVector.end(); ++stateVectorIter){
19 | 		position = 1;
20 | 		for(dataVec::iterator svIter = stateVectorIter->begin(); svIter != stateVectorIter->end(); ++svIter){
21 | 			if (position <= 3){
22 | 				outFile << *svIter << " ";
23 | 				position++;
24 | 			} else {continue;}
25 | 		}
26 | 		
27 | 		outFile << "\n";
28 | 	}
29 |  
30 |     outFile.close();
31 |   }
32 |   else std::cout << "Unable to open file";
33 | 
34 | 	
35 | }
36 | 


--------------------------------------------------------------------------------
/CPPFiles/quadStateEst.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * quadStateEst.cpp
  3 |  * Main Loop: Obtain Measurement, Feed into Kalman Filter, Present State
  4 |  * State Estimation of a Quadrotor for Autonomous Control of Flight
  5 |  * State: [Position, Velocity, Orientation, Accelerometer Bias, Roll/Pitch Bias at 0]
  6 |  * Fredy Monterroza 
  7 |  * 
  8 | */
  9 | 
 10 | #include <iostream>
 11 | #include <stdio.h>
 12 | #include <stdlib.h>
 13 | #include <string.h>
 14 | 
 15 | #include <math.h>
 16 | 
 17 | #include <boost/numeric/ublas/vector.hpp>
 18 | #include <boost/numeric/ublas/matrix.hpp>
 19 | #include <boost/numeric/ublas/io.hpp>
 20 | #include <boost/math/complex/acos.hpp>
 21 | #include <boost/math/complex/asin.hpp>
 22 | #include <boost/math/complex/atan.hpp>
 23 | #include <boost/random.hpp>
 24 | #include <boost/random/normal_distribution.hpp>
 25 | 
 26 | #include "inputData.cpp"
 27 | #include "fmUKF.cpp"
 28 | #include "outputDataUKF.cpp"
 29 | 
 30 | using std::cout;
 31 | using std::cin;
 32 | using std::endl;
 33 | 
 34 | using namespace boost::numeric;
 35 | 
 36 | //Data Containers from Sensor (IMU), nPointPose Measurement, Vicon
 37 | extern std::vector<dataVec> sensorLogAccImu;
 38 | extern std::vector<dataVec> sensorLogOmegaImu; 
 39 | extern std::vector<double> quadTimeLog;
 40 | extern std::vector<dataVec> visionRobotPos; 
 41 | extern std::vector<dataVec> visionRobotOrient; 
 42 | extern std::vector<dataVec> viconEuler; 
 43 | extern std::vector<dataVec> viconPos;
 44 | extern std::list<double> viconTime;
 45 | 
 46 | //Store State Estimates 
 47 | std::vector<dataVec> stateVector;
 48 | 
 49 | //Computation Variables
 50 | ublas::vector<double> Xest(14);
 51 | ublas::matrix<double> P(14,14);
 52 | double deltaT;
 53 | double prevT;
 54 | ublas::vector<double> Z(6);
 55 | ublas::vector<double> errVn(9);
 56 | ublas::vector<double> Vn(6);
 57 | 
 58 | 
 59 | //Process Covariance (Q) and Measurement Covariance (R)
 60 | ublas::matrix<double> Q(14,14);
 61 | ublas::matrix<double> R(6,6);
 62 | 
 63 | int kfIteration = 1;
 64 | 
 65 | int main(){
 66 | 	//For use with random walk model of accelerometer bias.
 67 | 	boost::mt19937 rng;
 68 | 	boost::normal_distribution<> xBias(0.0, 0.0094);
 69 |     boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > xBiasSamp(rng, xBias);
 70 |     boost::normal_distribution<> yBias(0.0,  0.0129);
 71 |     boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > yBiasSamp(rng, yBias);
 72 |     boost::normal_distribution<> zBias(0.0, 0.0120);
 73 |     boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > zBiasSamp(rng, zBias);
 74 | 	
 75 | 	inputData();
 76 | 	
 77 | 	//Fill Process Covariance Q
 78 | 	for(ublas::matrix<double>::iterator1 iter1Q = Q.begin1(); iter1Q != Q.end1(); ++iter1Q){ //Row by row
 79 | 		for(ublas::matrix<double>::iterator2 iter2Q = iter1Q.begin(); iter2Q != iter1Q.end(); ++iter2Q){ //Go from col to the next col for fixed row
 80 | 			if (iter1Q.index1() == iter2Q.index2()){
 81 | 				*iter2Q = .005;
 82 | 			} else {
 83 | 				*iter2Q = 0;
 84 | 			}
 85 | 		}
 86 | 	 } 
 87 | 	 
 88 | 	 //Fill Measurement Covariance R
 89 | 	 int traceIndex = 1;
 90 | 	 for(ublas::matrix<double>::iterator1 iter1R = R.begin1(); iter1R != R.end1(); ++iter1R){ //Row by row
 91 | 		for(ublas::matrix<double>::iterator2 iter2R = iter1R.begin(); iter2R != iter1R.end(); ++iter2R){ //Go from col to the next col for fixed row
 92 | 			if (iter1R.index1() == iter2R.index2()){
 93 | 				if (traceIndex <=3) { *iter2R = .2; }
 94 | 				else { *iter2R = .0001; }
 95 | 				traceIndex++;
 96 | 			}
 97 | 		}
 98 | 	 } 
 99 | 	 
100 | 	 
101 | 	 
102 | 	/*
103 | 	 * Initialize UKF
104 | 	 */ 
105 | 	
106 | 	//First State estimate formed from known onboard sensor values and pose estimate/transforations of N-Point Pose. (Refer to the Matlab source)
107 | 	Xest(0) = -0.197612876747667;
108 | 	Xest(1) = 0.079773798179542;
109 | 	Xest(2) = 0.873867606945072;
110 | 	Xest(3) = -0.361840566716804;
111 | 	Xest(4) = 0.364435964213824;
112 | 	Xest(5) = 0.107057884736583;
113 | 	Xest(6) = 0.003720102419965;
114 | 	Xest(7) = -.0003130927657911031;
115 | 	Xest(8) = 1.539666077119250;
116 | 	Xest(9) = 0.0;
117 | 	Xest(10) = 0.0;
118 | 	Xest(11) = 0.0;
119 | 	Xest(12) = 0.0;
120 | 	Xest(13) = 0.0;
121 | 	
122 | 	 
123 | 	//Initialize prevU: Previous Control Input to be used when data dropped. (Maintain Heading when Blind). 
124 | 	//Note: All the used control inputs already collected in sensorLogAccImu, sensorLogOmegaImu, simply iterate.
125 | 	
126 | 	//Initialize prevTime: First Time Stamp
127 | 	vector<double>::iterator qtIter = quadTimeLog.begin();
128 | 	prevT = *qtIter++;
129 | 	
130 | 	
131 | 	//Save the Position Estimate Component for use with RViz/3D trajectory visualization
132 | 	dataVec tempVec;
133 | 	for(unsigned i = 0; i < Xest.size(); ++i){
134 | 		tempVec.push_back(Xest(i));
135 | 	}
136 | 	stateVector.push_back(tempVec);
137 | 	
138 | 	//TODO: 2-Norm of Matrix P
139 | 	
140 | 	//First State Covariance Matrix P
141 | 	for(ublas::matrix<double>::iterator1 iter1P = P.begin1(); iter1P != P.end1(); ++iter1P){ //Row by row
142 | 		for(ublas::matrix<double>::iterator2 iter2P = iter1P.begin(); iter2P != iter1P.end(); ++iter2P){ //Go from col to the next col for fixed row
143 | 			if (iter1P.index1() == iter2P.index2()){
144 | 				*iter2P = .01;
145 | 			} else {
146 | 				*iter2P = 0;
147 | 			}
148 | 		}
149 | 	 } 
150 | 	 
151 | 	
152 | 	//Use Accelerometer Input Iterator as Marker for End of Loop
153 | 	vector<dataVec>::iterator accIter = sensorLogAccImu.begin();
154 | 	vector<dataVec>::iterator omegaIter = sensorLogOmegaImu.begin();
155 | 	vector<dataVec>::iterator pNPPIter = visionRobotPos.begin();
156 | 	vector<dataVec>::iterator oNPPIter = visionRobotOrient.begin();
157 | 	dataVec::iterator accInput;
158 | 	dataVec::iterator gyroInput;
159 | 	dataVec::iterator nppPos;
160 | 	dataVec::iterator nppOrient;
161 | 	++accIter; ++omegaIter; ++pNPPIter; ++oNPPIter; //First input/NPP unused here, but used in Matlab
162 | 	
163 | 	/*
164 | 	 * Unscented Kalman Filter Loop 
165 | 	 */ 
166 | 	
167 | 	while(accIter != sensorLogAccImu.end()){
168 | 		
169 | 		accInput = accIter->begin();
170 | 		gyroInput = omegaIter->begin();
171 | 		for(unsigned i = 0; i < (Vn.size()/2); ++i){
172 | 			Vn(i) = *accInput++;
173 | 			Vn(i+3) = *gyroInput++;
174 | 		}
175 | 		deltaT = *qtIter - prevT;
176 | 		errVn(0) = 0; errVn(1) = 0;  errVn(2) = 0;  errVn(3) = 0;  errVn(4) = 0;  errVn(5) = 0;  
177 | 		errVn(6) = xBiasSamp(); errVn(7) = yBiasSamp(); errVn(8) = zBiasSamp();
178 | 		
179 | 		nppPos = pNPPIter->begin();
180 | 		nppOrient = oNPPIter->begin();
181 | 		for(unsigned i = 0; i < (Z.size()/2); ++i){
182 | 			Z(i) = *nppPos++;
183 | 			Z(i+3) = *nppOrient++;
184 | 		}
185 | 		
186 | 		cout << kfIteration << endl;
187 | 		cout << Xest << endl;
188 | 		fmUKF(stateVector, Xest, P, deltaT, Z, errVn, Vn);
189 | 
190 | 		//Update prevTime
191 | 		prevT = *qtIter;
192 | 		
193 | 		//TODO: Save/Evaluate normP
194 | 		
195 | 		//Advance Iterators, kfIteration
196 | 		++qtIter; ++accIter; ++omegaIter; ++pNPPIter; ++oNPPIter;
197 | 		kfIteration++;
198 | 	}
199 | 	cout << "Number of Kalman Filter Iterations = " << kfIteration << endl;
200 | 	
201 | 	//Write Position Estimates to Text File
202 | 	outputDataUKF(stateVector);
203 | 	
204 | 	//Form Vector of Position for Visualization Purposes
205 | 	
206 | 	//Publish Data for use with RViz
207 | 	
208 | 
209 | 	return 0;
210 | 	
211 | }
212 | 
213 | 
214 | 
215 | 
216 | 


--------------------------------------------------------------------------------
/Data/poseNPPPos.txt:
--------------------------------------------------------------------------------
  1 | -0.197612876747667 0.079773798179542 0.873867606945072
  2 | -0.180847769465316 0.094999123811529 0.875462207344796
  3 | 0.000000000000000 0.000000000000000 0.000000000000000
  4 | -0.170135178399993 0.110794538724114 0.877978788726273
  5 | 0.000000000000000 0.000000000000000 0.000000000000000
  6 | -0.155949340275242 0.123972863884249 0.883341051560189
  7 | 0.000000000000000 0.000000000000000 0.000000000000000
  8 | 0.000000000000000 0.000000000000000 0.000000000000000
  9 | -0.157623841143130 0.128987356454868 0.888424085488361
 10 | -0.154841246881759 0.203771335312668 0.874299276399685
 11 | 0.000000000000000 0.000000000000000 0.000000000000000
 12 | -0.141181325919422 0.183121230199882 0.864936250738617
 13 | 0.000000000000000 0.000000000000000 0.000000000000000
 14 | 0.000000000000000 0.000000000000000 0.000000000000000
 15 | -0.124734488028096 0.202917427618437 0.865324448278436
 16 | 0.000000000000000 0.000000000000000 0.000000000000000
 17 | 0.000000000000000 0.000000000000000 0.000000000000000
 18 | 0.000000000000000 0.000000000000000 0.000000000000000
 19 | 0.000000000000000 0.000000000000000 0.000000000000000
 20 | 0.000000000000000 0.000000000000000 0.000000000000000
 21 | -0.099317273963409 0.238245215888411 0.864516074311141
 22 | -0.088286645982265 0.251903568549760 0.863978125265729
 23 | -0.076796072631769 0.266746180062597 0.862230402546812
 24 | 0.000000000000000 0.000000000000000 0.000000000000000
 25 | -0.063253301281654 0.284352431235769 0.860301252707904
 26 | -0.051967481396621 0.301191438654473 0.859031044799463
 27 | -0.043133072672500 0.316688650177724 0.857863266345989
 28 | 0.000000000000000 0.000000000000000 0.000000000000000
 29 | 0.000000000000000 0.000000000000000 0.000000000000000
 30 | -0.027816328356774 0.333839948649863 0.853701994059227
 31 | -0.017157288692353 0.346654972945586 0.852286892158756
 32 | -0.000052310962474 0.366577696741824 0.851328431845302
 33 | 0.000000000000000 0.000000000000000 0.000000000000000
 34 | 0.002967606798917 0.379440645097337 0.847458615641017
 35 | 0.012755442749290 0.396923652815719 0.845728052416530
 36 | 0.019253806079024 0.412123861392697 0.843311820714442
 37 | 0.000000000000000 0.000000000000000 0.000000000000000
 38 | 0.000000000000000 0.000000000000000 0.000000000000000
 39 | 0.000000000000000 0.000000000000000 0.000000000000000
 40 | 0.000000000000000 0.000000000000000 0.000000000000000
 41 | 0.000000000000000 0.000000000000000 0.000000000000000
 42 | 0.024008515537250 0.449924423397742 0.838091584727842
 43 | 0.031450072727538 0.469267113419758 0.835052887175713
 44 | 0.036759485836482 0.487785436037118 0.831572518321305
 45 | 0.044084913205640 0.499617638223334 0.827187096784836
 46 | 0.048950750092765 0.517014378133264 0.824937765540099
 47 | 0.052536587380044 0.534750012068309 0.822252085493174
 48 | 0.047035496472809 0.546512951123732 0.818175616118754
 49 | 0.047368493831208 0.562190853484492 0.815796349400301
 50 | 0.047146821629518 0.576347521466864 0.813380166982906
 51 | 0.047261416051423 0.591068466062553 0.812544231486722
 52 | 0.046134668232440 0.606310443495117 0.812341577879133
 53 | 0.044502486400341 0.619876320540184 0.811994108431144
 54 | 0.042375917049147 0.633789832994129 0.811856137317794
 55 | 0.038139259574872 0.646294346429669 0.811939919904070
 56 | 0.032048372582629 0.655325179541842 0.812476270302361
 57 | 0.000000000000000 0.000000000000000 0.000000000000000
 58 | 0.025837484769274 0.678784715247147 0.815039392802798
 59 | 0.023371974571764 0.692130055877390 0.817867629305929
 60 | 0.000000000000000 0.000000000000000 0.000000000000000
 61 | 0.022285036047928 0.707125513348734 0.820572602485314
 62 | 0.018282666701449 0.717650447249865 0.823041657158274
 63 | 0.015772388195313 0.730312222207820 0.826303681694949
 64 | 0.011608341117142 0.742375481802557 0.829588395921426
 65 | 0.005802383886848 0.754548373893918 0.833874727313391
 66 | 0.001055221595698 0.766195381517467 0.838227947641841
 67 | -0.002617123264728 0.778672321394905 0.841697290504178
 68 | -0.006354327437608 0.790434206882719 0.845965100089012
 69 | 0.000000000000000 0.000000000000000 0.000000000000000
 70 | -0.011462056640909 0.802161147014805 0.851295632951107
 71 | -0.014612714026702 0.814898310661873 0.856035641474582
 72 | -0.018195107274435 0.828033837044854 0.861417532117859
 73 | 0.000000000000000 0.000000000000000 0.000000000000000
 74 | 0.000000000000000 0.000000000000000 0.000000000000000
 75 | 0.000000000000000 0.000000000000000 0.000000000000000
 76 | 0.000000000000000 0.000000000000000 0.000000000000000
 77 | -0.030612359725510 0.849873713635525 0.874875630147470
 78 | -0.035190258069073 0.863613216200371 0.881253167141801
 79 | -0.037197979148666 0.879440585909792 0.887146717385393
 80 | 0.000000000000000 0.000000000000000 0.000000000000000
 81 | 0.000000000000000 0.000000000000000 0.000000000000000
 82 | -0.042003529323251 0.891278887123833 0.893752594416311
 83 | -0.043787535045185 0.905401156965007 0.901573411405190
 84 | -0.044206342897662 0.920726902659042 0.908381979124311
 85 | -0.047228179769419 0.933756862825015 0.915933448439381
 86 | -0.041171517105700 0.951918642659800 0.923582802543047
 87 | -0.042140840019100 0.958409369939951 0.935852285017672
 88 | -0.041745164274239 0.975142293007020 0.939845047188614
 89 | -0.041756248801957 0.989505815123897 0.948719135254141
 90 | -0.040670591763974 1.003943608578908 0.958432563834865
 91 | 0.000000000000000 0.000000000000000 0.000000000000000
 92 | 0.000000000000000 0.000000000000000 0.000000000000000
 93 | -0.039974077061522 1.017288508504881 0.968126887647250
 94 | -0.040530944805599 1.029859533066529 0.978412301951806
 95 | -0.039068070035285 1.043313273295044 0.989431845306546
 96 | 0.000000000000000 0.000000000000000 0.000000000000000
 97 | -0.037653248188525 1.057363144802930 0.999801232430025
 98 | -0.035678671550739 1.070779128520882 1.010682835650714
 99 | -0.034965892920055 1.082863651059547 1.022356741492512
100 | -0.031141084270704 1.097750434062449 1.033700440764312
101 | -0.023944107610627 1.112009820136261 1.045243852537681
102 | 0.000000000000000 0.000000000000000 0.000000000000000
103 | -0.015760171151788 1.126370617215538 1.056928130287922
104 | -0.003886413902816 1.143492749256714 1.068419026197714
105 | -0.014371669927976 1.138359691576669 1.080860504876985
106 | -0.003394345375134 1.161773268893443 1.090301949922036
107 | 0.008304529550272 1.176366436637001 1.101706984526455
108 | 0.020061892746939 1.194206567762487 1.115524905897482
109 | 0.027047798133757 1.207807142056458 1.128694604073670
110 | 0.036365829380272 1.221731335599108 1.142628390622697
111 | 0.044241119996158 1.236060854269823 1.155921214655384
112 | 0.053646156222401 1.250563650367539 1.169629864829085
113 | 0.061187976995786 1.264964042774709 1.183726972221004
114 | 0.067405646420704 1.278373747716483 1.197563533793766
115 | 0.076513980388809 1.292027174998778 1.211824082053666
116 | 0.082820161984282 1.303628101815933 1.227475557279666
117 | 0.000000000000000 0.000000000000000 0.000000000000000
118 | 0.000000000000000 0.000000000000000 0.000000000000000
119 | 0.000000000000000 0.000000000000000 0.000000000000000
120 | 0.000000000000000 0.000000000000000 0.000000000000000
121 | 0.098235231923480 1.335728683371175 1.256736319724812
122 | 0.110188752641822 1.357021878450228 1.270405707974094
123 | 0.000000000000000 0.000000000000000 0.000000000000000
124 | 0.000000000000000 0.000000000000000 0.000000000000000
125 | 0.000000000000000 0.000000000000000 0.000000000000000
126 | 0.120254406172363 1.378926536863746 1.296639645586360
127 | 0.128032183415826 1.394473803421965 1.311026312368432
128 | 0.134077848793055 1.408243400374865 1.324152121521072
129 | 0.000000000000000 0.000000000000000 0.000000000000000
130 | 0.000000000000000 0.000000000000000 0.000000000000000
131 | 0.000000000000000 0.000000000000000 0.000000000000000
132 | 0.146213677114085 1.432416581703868 1.349856339899622
133 | 0.154794227473343 1.449150824123618 1.361556519732901
134 | 0.160306511461350 1.466551624037514 1.372776655201384
135 | 0.168703530412134 1.481752342904663 1.384714882132388
136 | 0.178840490661352 1.497573651844089 1.395532759994902
137 | 0.000000000000000 0.000000000000000 0.000000000000000
138 | 0.194793738678218 1.526189947148337 1.416293026543831
139 | 0.000000000000000 0.000000000000000 0.000000000000000
140 | 0.000000000000000 0.000000000000000 0.000000000000000
141 | 0.000000000000000 0.000000000000000 0.000000000000000
142 | 0.000000000000000 0.000000000000000 0.000000000000000
143 | 0.000000000000000 0.000000000000000 0.000000000000000
144 | 0.220812032083785 1.570659903802058 1.446997806606875
145 | 0.231952759531018 1.585547972082395 1.455826293335704
146 | 0.238628887063397 1.599684089314865 1.464519016870794
147 | 0.248720885927691 1.614648113230437 1.473152269357782
148 | 0.000000000000000 0.000000000000000 0.000000000000000
149 | 0.256906480582060 1.628988068695584 1.481626688147019
150 | 0.265717036618013 1.643083923681996 1.490114957995223
151 | 0.275952310097057 1.657590173447015 1.498819495651841
152 | 0.282578477832447 1.669311584675552 1.507240404051902
153 | 0.291402189773490 1.683281594742057 1.514954920084729
154 | 0.304802273551007 1.708084739109627 1.531549229017509
155 | 0.315360999486872 1.722938624863930 1.539767601864938
156 | 0.325097308550295 1.735489764291752 1.547366434664909
157 | 0.334665197406264 1.746899477926537 1.555162744960640
158 | 0.346971357868363 1.762200403570863 1.563152256743354
159 | 0.000000000000000 0.000000000000000 0.000000000000000
160 | 0.366757758176789 1.786657695166987 1.577818090258915
161 | 0.000000000000000 0.000000000000000 0.000000000000000
162 | 0.377658240000258 1.798637495254716 1.585204364991326
163 | 0.388852683114358 1.810447417208104 1.592483669166901
164 | 0.400203776019422 1.822700919561492 1.599823883376135
165 | 0.423064661895751 1.847318586491616 1.616049341098985
166 | 0.434422093574125 1.859798204632324 1.623844826290247
167 | 0.447146710052604 1.869221053840731 1.630569976342199
168 | 0.460358199529335 1.878915719199881 1.637487276331507
169 | 0.474628484758201 1.889859381342480 1.642448250852316
170 | 0.492473290258364 1.901609550244482 1.649672039976883
171 | 0.520900695280536 1.919657863997775 1.662263059006056
172 | 0.536213090963937 1.925897772137811 1.667111668275059
173 | 0.549311236087815 1.925238129974114 1.677066210081327
174 | 0.570594821108062 1.938967660690852 1.682087411043214
175 | 0.591079976521711 1.951822880276278 1.686649002673341
176 | 0.612789072331079 1.956265802467144 1.691917796281696
177 | 0.629904754875753 1.968052866793804 1.698899586648908
178 | 0.648921105663416 1.966134795076188 1.703892321936700
179 | 0.676022593061893 1.971373945048599 1.710339019063941
180 | 0.695011441821045 1.979546697926812 1.716626238913589
181 | 0.726051332292806 1.979049807616904 1.721471545212227
182 | 0.751317274276492 1.978218207153865 1.727228430904192
183 | 0.772293170228150 1.975435372258941 1.733160637066536
184 | 0.795201462810971 1.973817336903391 1.738730842209926
185 | 0.821347464356123 1.974956739285032 1.743553091887900
186 | 0.846849779893672 1.971949366656277 1.748772307553076
187 | 0.000000000000000 0.000000000000000 0.000000000000000
188 | 0.000000000000000 0.000000000000000 0.000000000000000
189 | 0.839230293462836 1.952466400960849 1.756248960968898
190 | 0.000000000000000 0.000000000000000 0.000000000000000
191 | 0.000000000000000 0.000000000000000 0.000000000000000
192 | 0.893945749160084 1.973469188741100 1.756722185963343
193 | 0.000000000000000 0.000000000000000 0.000000000000000
194 | 0.000000000000000 0.000000000000000 0.000000000000000
195 | 0.000000000000000 0.000000000000000 0.000000000000000
196 | 1.286373116911930 1.921205263558059 1.778750247728624
197 | 0.000000000000000 0.000000000000000 0.000000000000000
198 | 0.000000000000000 0.000000000000000 0.000000000000000
199 | 0.518296802018118 2.299347731464286 2.099508643692399
200 | 0.975479794013903 1.969418121357670 1.767651884009475
201 | 1.001271679677894 1.958394246538751 1.769748612183641
202 | 1.021940045553679 1.955201552155842 1.770621913503583
203 | 1.048593833754852 1.954794032006984 1.772841939449586
204 | 1.072824790507773 1.952920525964237 1.774258632866118
205 | 1.098213775656214 1.951119377831489 1.775526630218076
206 | 1.122186884670622 1.949950533373680 1.776924598647045
207 | 1.154328928985806 1.948178959394710 1.778519995887860
208 | 1.179498319782725 1.947770826783352 1.778422477845498
209 | 1.205463151773138 1.946788867184154 1.777667473761945
210 | 1.308839002822054 1.948462267660768 1.781459273060789
211 | 1.363718260675860 1.956380587798834 1.779798054982333
212 | 1.387680782649379 1.957341462526146 1.779562067263975
213 | 1.414137875588153 1.962424599983035 1.777914087867697
214 | 1.468050830572014 1.963911745509445 1.778064450774958
215 | 1.492340535454655 1.956928152751674 1.778501786914054
216 | 0.000000000000000 0.000000000000000 0.000000000000000
217 | 0.000000000000000 0.000000000000000 0.000000000000000
218 | 0.000000000000000 0.000000000000000 0.000000000000000
219 | 1.542525148142395 1.954899022273471 1.778631677953401
220 | 1.563848499916049 1.949282444835075 1.779443269461117
221 | 1.618278417108015 1.947579386326748 1.779423374354951
222 | 1.642322947182306 1.943824325589202 1.779220724273297
223 | 1.664430968396375 1.935252302023569 1.780539658011225
224 | 1.706779737258531 1.931571292359606 1.781579990148111
225 | 1.730960761877257 1.926680376886690 1.782714413477873
226 | 1.752436351797352 1.921962395282788 1.782546587481745
227 | 1.774763158326425 1.911535452602401 1.782388231569218
228 | 1.796913089376732 1.911374135604013 1.783380623674987
229 | 1.817153119810960 1.907581787905998 1.783465751083344
230 | 1.837311366822203 1.896135796867410 1.782960044185961
231 | 1.856141542259445 1.884395645164912 1.784107746451844
232 | 1.876159969301525 1.878330021102100 1.784627775817328
233 | 1.901573047387323 1.880294200783778 1.786435841924974
234 | 0.000000000000000 0.000000000000000 0.000000000000000
235 | 0.000000000000000 0.000000000000000 0.000000000000000
236 | 0.000000000000000 0.000000000000000 0.000000000000000
237 | 0.000000000000000 0.000000000000000 0.000000000000000
238 | 1.962862665583903 1.845992823822751 1.787718700247685
239 | 1.979318454733523 1.834255789116734 1.787994992497025
240 | 1.993017738634075 1.824377825588375 1.788866979819514
241 | 2.029666102270496 1.814849873742415 1.788615829300523
242 | 2.046027850963310 1.803860573660083 1.789522204726594
243 | 2.062414629965460 1.797351197865480 1.788788099389162
244 | 2.074999818566955 1.792373380288794 1.787436442267808
245 | 2.087375475431900 1.781438797083655 1.785136874556506
246 | 2.110508535658996 1.766374564207765 1.783568896535665
247 | 2.122282420455065 1.758171902932241 1.781898505017318
248 | 2.128138495489297 1.744489964328966 1.781187235226960
249 | 2.135704286088527 1.740904221341704 1.779573278987471
250 | 2.142796597581047 1.733098800837337 1.777969456889548
251 | 2.150250289542761 1.714659038373868 1.775835738842125
252 | 2.153662526968227 1.707569053502703 1.773231593318961
253 | 2.156268017225461 1.695484514960969 1.771821359098461
254 | 2.160675682437085 1.685094819688948 1.771136998492404
255 | 2.158994862601168 1.665480305054533 1.767453759117625
256 | 0.000000000000000 0.000000000000000 0.000000000000000
257 | 0.000000000000000 0.000000000000000 0.000000000000000
258 | 0.000000000000000 0.000000000000000 0.000000000000000
259 | 2.158227588427812 1.645196467457628 1.764118662500023
260 | 2.152256094038797 1.631477782652684 1.761409247711140
261 | 2.150213108173398 1.619683058091918 1.759888595832755
262 | 2.145454793848858 1.609475348417805 1.756913148350331
263 | 2.138215705868714 1.595055500158793 1.753612504063646
264 | 2.134834949397265 1.584174548131039 1.751836628022364
265 | 2.128251650657660 1.573455644417886 1.749049279508915
266 | 2.123181933360493 1.557787223618031 1.747162269026605
267 | 2.108501062435528 1.531701831597259 1.740388121819152
268 | 2.092668754805774 1.498141886667616 1.735170355370237
269 | 2.082865062439204 1.482459515784171 1.732272207167878
270 | 0.000000000000000 0.000000000000000 0.000000000000000
271 | 0.000000000000000 0.000000000000000 0.000000000000000
272 | 2.079044230087534 1.467145612826455 1.729711497151612
273 | 2.070734746615270 1.452008150821641 1.726960849352953
274 | 2.063444166196485 1.433451663206900 1.723928852674856
275 | 2.057035293397692 1.417806202360066 1.721528606963770
276 | 2.048428686440810 1.401116603215554 1.717404859037179
277 | 2.044914286387044 1.385130575022962 1.714007257284657
278 | 2.028680304099262 1.344323218216077 1.705716417685133
279 | 0.000000000000000 0.000000000000000 0.000000000000000
280 | 0.000000000000000 0.000000000000000 0.000000000000000
281 | 2.016897896590649 1.308959769766814 1.701380559807981
282 | 2.007430093437174 1.291620178940565 1.699134980757499
283 | 1.998017699584939 1.265228526765851 1.695139983025755
284 | 1.991870177016527 1.248759950555678 1.692548295898848
285 | 1.981244021417614 1.231012999917544 1.689146482227048
286 | 1.973951965723431 1.212764255912880 1.685471159133964
287 | 1.954799192558015 1.178272925613941 1.679892326020748
288 | 0.000000000000000 0.000000000000000 0.000000000000000
289 | 1.937105029654441 1.142946294832385 1.672442132050291
290 | 1.928632487040511 1.128473872499085 1.669090049215915
291 | 1.914218377189013 1.112527054001857 1.663926173116878
292 | 1.904721200592945 1.096640809582536 1.660820761317830
293 | 1.880921324567997 1.067279912632099 1.652868380186988
294 | 1.872448397400933 1.053474762790246 1.648633077341987
295 | 1.862165909542336 1.035757643371695 1.646268805713534
296 | 1.850892120572175 1.023343108082464 1.642188662618642
297 | 1.825432956001181 0.994723388694484 1.634856856168593
298 | 1.815483573645189 0.982794178315437 1.630593663932176
299 | 1.797303453534523 0.969843976045245 1.626888270296454
300 | 1.785015978938160 0.955393419928151 1.622902224850359
301 | 1.757433554320706 0.931011431148398 1.615418188880207
302 | 1.740895658251654 0.917953630005284 1.611867634274088
303 | 0.000000000000000 0.000000000000000 0.000000000000000
304 | 0.000000000000000 0.000000000000000 0.000000000000000
305 | 0.000000000000000 0.000000000000000 0.000000000000000
306 | 1.728806613153393 0.908028421410158 1.608127700768165
307 | 1.713596896136107 0.897929747617525 1.603748662286915
308 | 1.696425049314444 0.885465020636567 1.601190603327739
309 | 1.664669547191354 0.865715314167837 1.592604108990308
310 | 0.000000000000000 0.000000000000000 0.000000000000000
311 | 0.000000000000000 0.000000000000000 0.000000000000000
312 | 0.000000000000000 0.000000000000000 0.000000000000000
313 | 0.000000000000000 0.000000000000000 0.000000000000000
314 | 1.644252817632897 0.852581919896797 1.587767223957105
315 | 1.627041065837184 0.842469428377738 1.584057965967589
316 | 1.609307964264678 0.831862400937484 1.580510452371627
317 | 1.568908212982011 0.812722272770546 1.571680626794771
318 | 1.524450219328914 0.794172060932050 1.562751871021912
319 | 1.500213444089476 0.784613630265410 1.556713595725325
320 | 0.000000000000000 0.000000000000000 0.000000000000000
321 | 0.000000000000000 0.000000000000000 0.000000000000000
322 | 0.000000000000000 0.000000000000000 0.000000000000000
323 | 1.460273084945335 0.771500916490751 1.549941486862076
324 | 1.443256664697849 0.767506135643929 1.545118666671064
325 | 1.420685011483844 0.761001114218257 1.540281283712121
326 | 1.400498502257327 0.755771185334401 1.536092539399460
327 | 0.000000000000000 0.000000000000000 0.000000000000000
328 | 0.000000000000000 0.000000000000000 0.000000000000000
329 | 1.356538820581933 0.745746756609765 1.527004154765811
330 | 0.000000000000000 0.000000000000000 0.000000000000000
331 | 0.000000000000000 0.000000000000000 0.000000000000000
332 | 0.000000000000000 0.000000000000000 0.000000000000000
333 | 0.000000000000000 0.000000000000000 0.000000000000000
334 | 0.000000000000000 0.000000000000000 0.000000000000000
335 | 0.000000000000000 0.000000000000000 0.000000000000000
336 | 0.000000000000000 0.000000000000000 0.000000000000000
337 | 0.000000000000000 0.000000000000000 0.000000000000000
338 | 0.000000000000000 0.000000000000000 0.000000000000000
339 | 0.000000000000000 0.000000000000000 0.000000000000000
340 | 0.000000000000000 0.000000000000000 0.000000000000000
341 | 1.266812833155140 0.732480183726883 1.511198133691121
342 | 1.243977738916882 0.729621434959233 1.508043934639460
343 | 1.220996131566030 0.727275760006715 1.504031782280078
344 | 1.197552415900670 0.726335151348377 1.499591843192302
345 | 1.145804741437177 0.721926722649641 1.492635693472560
346 | 1.122155425751881 0.721181106070030 1.489290098613177
347 | 1.100126938001612 0.723141224925458 1.485256683453435
348 | 1.072467058968981 0.724086314971291 1.480785433538856
349 | 1.045146468877270 0.722849983697448 1.477404312220991
350 | 0.992010911256370 0.726250588091641 1.469777965585372
351 | 0.963690274659900 0.724233386532461 1.466828618436886
352 | 0.934333111153224 0.727138290200403 1.462823521153878
353 | 0.903279625393664 0.727479201345907 1.459730227912236
354 | 0.878195927354842 0.733083490314930 1.456233210626727
355 | 0.846790029234748 0.735214586393789 1.452166685731637
356 | 0.816968530714720 0.739282105986201 1.448464524699791
357 | 0.000000000000000 0.000000000000000 0.000000000000000
358 | 0.000000000000000 0.000000000000000 0.000000000000000
359 | 0.789246849981489 0.741659879232541 1.444583346273650
360 | 0.754470432250275 0.746427352841224 1.440442440160195
361 | 0.727924489030612 0.754302574051197 1.436290644092073
362 | 0.663006962180227 0.760719455896826 1.428481489864110
363 | 0.608404802897939 0.773648415386677 1.420404223332163
364 | 0.580270237100808 0.780330550367665 1.416918161082851
365 | 0.516750974498860 0.794737278102297 1.407481794623780
366 | 0.490274374665025 0.802412008571327 1.403209086692315
367 | 0.440576193509361 0.817834618487237 1.394149311916776
368 | 0.393874795975292 0.838957251047694 1.383477136429557
369 | 0.371520183109731 0.848008412465862 1.379517056271908
370 | 0.330679841403212 0.869089333364011 1.368586612356752
371 | 0.309752875715049 0.880021724585333 1.363131151010219
372 | 0.294103637925613 0.891015248396892 1.357299623853008
373 | 0.280236545296911 0.903589171880816 1.352107555783863
374 | 0.265717013445025 0.916676436694223 1.346804169452244
375 | 0.255530450635865 0.928776972442875 1.340748154145807
376 | 0.241827144104632 0.938965294940126 1.336076378276167
377 | 0.234446665710445 0.950061299749515 1.330926359544408
378 | 0.228809994421892 0.962478019452981 1.324581628903497
379 | 0.223304587988252 0.973521994496515 1.319042925168939
380 | 0.222938990868426 0.981881545179527 1.313783926022433
381 | 0.218725551817941 0.992548513350300 1.308646001725426
382 | 0.218318008797970 1.005314033211163 1.302817326171736
383 | 0.217043441611746 1.016107513576086 1.297121717107179
384 | 0.216791982761317 1.028466283307652 1.291734425215414
385 | 0.214291020053958 1.039455427222394 1.286630117963935
386 | 0.220183867978204 1.064686340198918 1.274618514061514
387 | 0.220977803817001 1.074215545019630 1.269048488154238
388 | 0.226368431944745 1.086922490519104 1.263716914383952
389 | 0.229209018424645 1.100522229082931 1.258804874035251
390 | 0.238730283217518 1.114917389463957 1.252956995476802
391 | 0.000000000000000 0.000000000000000 0.000000000000000
392 | 0.247104243151153 1.128585621915051 1.247928231303606
393 | 0.256319533717615 1.142220691398653 1.242917706674280
394 | 0.264678709886501 1.155018210577493 1.238450649669730
395 | 0.274998837240822 1.169442458890364 1.233507440620368
396 | 0.283809457679349 1.183605444751672 1.229277305700260
397 | 0.293372245961786 1.198412883446532 1.225094810250568
398 | 0.312037791123185 1.227904800405940 1.216243688255878
399 | 0.323441625950639 1.244172648636231 1.212149103287179
400 | 0.331533584786226 1.258401569030831 1.208224602933864
401 | 0.340735068513187 1.273917105290548 1.204855216296365
402 | 0.350798749706182 1.288954523725848 1.201749723790222
403 | 0.359536515432467 1.301679994031358 1.199686285523366
404 | 0.370850951361641 1.318945474200374 1.196352897891290
405 | 0.382315460072790 1.335731115211481 1.192755620855340
406 | 0.394508600809481 1.355817772610659 1.188978407202095
407 | 0.401539504396626 1.361297587376051 1.187034668900363
408 | 0.421669958448978 1.398794471653543 1.181314711915065
409 | 0.430827818549645 1.414928988141062 1.180159214513268
410 | 0.000000000000000 0.000000000000000 0.000000000000000
411 | 0.000000000000000 0.000000000000000 0.000000000000000
412 | 0.451379415643251 1.450039173747531 1.178092260419003
413 | 0.461022613832871 1.465863166321999 1.176801301519641
414 | 0.471456389081852 1.484498104934472 1.175263401714557
415 | 0.481008614363016 1.502734122242867 1.175030099568780
416 | 0.490545529262290 1.522051825740714 1.175108217618948
417 | 0.500297845435540 1.543389267651379 1.176079951974959
418 | 0.511996100489660 1.575468882030896 1.175275159859854
419 | 0.509244547489218 1.591852294373442 1.174885164785955
420 | 0.517656717911393 1.609336626096310 1.176285677580871
421 | 0.000000000000000 0.000000000000000 0.000000000000000
422 | 0.000000000000000 0.000000000000000 0.000000000000000
423 | 0.525774487567085 1.630197822652137 1.176215786557287
424 | 0.532068044377449 1.650021336683767 1.176011006019282
425 | 0.544235514927842 1.672385778169986 1.176339947456284
426 | 0.549824514586271 1.691421736760702 1.177353273921057
427 | 0.560750537477697 1.731627960158242 1.179762434605521
428 | 0.568536219487649 1.757568247568609 1.182260312901982
429 | 0.574048762328577 1.777205372429345 1.184476302452536
430 | 0.587760841685911 1.815434797489943 1.190587327359259
431 | 0.593475574805939 1.833523369534481 1.193230260925321
432 | 0.600340612614429 1.851524431028482 1.196178535586288
433 | 0.606921697015428 1.872745272184207 1.201576201218388
434 | 0.615576937661080 1.906942509914307 1.207070582852340
435 | 0.621622773275645 1.924516318942516 1.209678580121269
436 | 0.629118364165360 1.941864169388915 1.213036144919383
437 | 0.000000000000000 0.000000000000000 0.000000000000000
438 | 0.000000000000000 0.000000000000000 0.000000000000000
439 | 0.000000000000000 0.000000000000000 0.000000000000000
440 | 0.643321590518932 1.974925578459055 1.219644727051056
441 | 0.650026176086253 1.992924047915797 1.224515444084309
442 | 0.656797144227193 2.005366070619321 1.226876722831996
443 | 0.665466482240855 2.021858100351572 1.231588582968481
444 | 0.673774502649948 2.037248075150383 1.236294098594684
445 | 0.685326562586422 2.059579008466836 1.246444774348421
446 | 0.000000000000000 0.000000000000000 0.000000000000000
447 | 0.000000000000000 0.000000000000000 0.000000000000000
448 | 0.000000000000000 0.000000000000000 0.000000000000000
449 | 0.694481692766845 2.072005661020281 1.251517537436204
450 | 0.000000000000000 0.000000000000000 0.000000000000000
451 | 0.000000000000000 0.000000000000000 0.000000000000000
452 | 0.000000000000000 0.000000000000000 0.000000000000000
453 | 0.000000000000000 0.000000000000000 0.000000000000000
454 | 0.000000000000000 0.000000000000000 0.000000000000000
455 | 0.704378902627205 2.083083566472133 1.257063752086837
456 | 0.000000000000000 0.000000000000000 0.000000000000000
457 | 0.000000000000000 0.000000000000000 0.000000000000000
458 | 0.000000000000000 0.000000000000000 0.000000000000000
459 | 0.000000000000000 0.000000000000000 0.000000000000000
460 | 0.000000000000000 0.000000000000000 0.000000000000000
461 | 0.000000000000000 0.000000000000000 0.000000000000000
462 | 0.000000000000000 0.000000000000000 0.000000000000000
463 | 0.713701280816310 2.093176188263385 1.262766048961926
464 | 0.722923179088511 2.101990524965559 1.269308637820141
465 | 0.740901363989691 2.117265678760047 1.283277277208070
466 | 0.753102924138687 2.125099512374502 1.288760704434944
467 | 0.764499297366930 2.133896234885727 1.295088550315977
468 | 0.773677546195927 2.139285632082396 1.301683657493463
469 | 0.796157606472563 2.148276528206043 1.315685983752338
470 | 0.806304659345209 2.150192748851953 1.322923982079669
471 | 0.840745408353058 2.149952896796743 1.346458380993331
472 | 0.000000000000000 0.000000000000000 0.000000000000000
473 | 0.000000000000000 0.000000000000000 0.000000000000000
474 | 0.870236002850007 2.153408001758979 1.363079488271510
475 | 0.881612335948576 2.152638233243017 1.371456650105120
476 | 0.913140836285276 2.154838936522310 1.389122997376737
477 | 0.928116620076986 2.156198370884457 1.397561254088014
478 | 0.942620790950104 2.156889977329932 1.405787495443351
479 | 0.000000000000000 0.000000000000000 0.000000000000000
480 | 0.000000000000000 0.000000000000000 0.000000000000000
481 | 0.957781734686963 2.158961963381845 1.413138133673671
482 | 0.000000000000000 0.000000000000000 0.000000000000000
483 | 0.990498973144982 2.159863804483376 1.430602313647829
484 | 1.003714188172392 2.156732447116533 1.438461000603715
485 | 1.027358296154475 2.159165602311505 1.449560238369328
486 | 1.044891800143846 2.157877148654689 1.457791821484542
487 | 1.063632986227341 2.158846316648733 1.465366684473739
488 | 1.081549304741765 2.158726205947631 1.472873411690323
489 | 1.100841660932900 2.163009424192737 1.479275156510205
490 | 1.120919269703867 2.163399654369838 1.486147909937982
491 | 1.138363173385643 2.160308644207269 1.493293454995005
492 | 1.181190969407351 2.159787342874502 1.506672729508719
493 | 1.203751674060122 2.160840480183929 1.512373114327630
494 | 0.000000000000000 0.000000000000000 0.000000000000000
495 | 0.000000000000000 0.000000000000000 0.000000000000000
496 | 0.000000000000000 0.000000000000000 0.000000000000000
497 | 0.000000000000000 0.000000000000000 0.000000000000000
498 | 0.000000000000000 0.000000000000000 0.000000000000000
499 | 1.247770380126170 2.154567915128512 1.522882141063119
500 | 1.271184373556451 2.153309672009678 1.527335444688118
501 | 1.298866163403051 2.153501302176674 1.531757236914190
502 | 0.000000000000000 0.000000000000000 0.000000000000000
503 | 0.000000000000000 0.000000000000000 0.000000000000000
504 | 1.353216462768761 2.150957353825340 1.539912681985445
505 | 1.383767047607973 2.148842185743688 1.543717566114476
506 | 0.000000000000000 0.000000000000000 0.000000000000000
507 | 0.000000000000000 0.000000000000000 0.000000000000000
508 | 1.435779115113557 2.148358196195425 1.550819277858206
509 | 1.494645677638503 2.137421007907022 1.560715396463338
510 | 1.547612026824655 2.153379559294392 1.563079522327300
511 | 0.000000000000000 0.000000000000000 0.000000000000000
512 | 0.000000000000000 0.000000000000000 0.000000000000000
513 | 1.607298791987025 2.133919799913140 1.568979852117055
514 | 1.621239757767690 2.084215634094908 1.577124771664289
515 | 1.626501720823471 2.536884940063802 1.722192872784356
516 | 1.736697771277598 2.123751779507185 1.584574232418613
517 | 1.789083498630072 2.123784560034536 1.585510891368542
518 | 1.820378197528034 2.121891941943983 1.588051493875792
519 | 1.860370867633733 2.127692698183645 1.590103438413344
520 | 0.000000000000000 0.000000000000000 0.000000000000000
521 | 0.000000000000000 0.000000000000000 0.000000000000000
522 | 1.955253937400167 2.127825004253372 1.599872830588760
523 | 1.992500869849372 2.131773868805853 1.602008356758855
524 | 2.028879933560389 2.131091570108704 1.603085297228820
525 | 2.065174453066476 2.134497693261572 1.605160630837887
526 | 2.098293867158273 2.135281418972433 1.608266692619734
527 | 2.124517506164006 2.125492037943859 1.609974672953794
528 | 2.163238841283816 2.117608425333995 1.614513281819642
529 | 2.193462692508972 2.108459582307238 1.618467629722422
530 | 2.242410879287996 2.119878228967672 1.622049187861430
531 | 2.273197127982091 2.122643652518299 1.624514262707327
532 | 2.302253415843817 2.117836405815638 1.628970355771345
533 | 2.330460655541377 2.108761042469203 1.634224133809121
534 | 2.354024014975405 2.104610378091057 1.637667774832086
535 | 2.378353549638782 2.100193271302872 1.641380652863008
536 | 2.400492138592622 2.093917720394264 1.645055624522308
537 | 2.421659282018066 2.084960398875838 1.650597075436598
538 | 2.440774453597326 2.075482114796768 1.655326286637903
539 | 2.460409030524740 2.073932882736058 1.660948473611178
540 | 2.480072213359404 2.065561591088118 1.665774001419790
541 | 2.518897754840668 2.044260797642577 1.677333507167135
542 | 2.533738130578937 2.031898620968254 1.680270395499343
543 | 2.547314563421940 2.022177689014065 1.686531495847646
544 | 2.559914810796255 2.008549608210893 1.691763773986634
545 | 2.574775229524567 1.997726734710640 1.695218528262205
546 | 2.587317034446853 1.981755918844982 1.699413718467246
547 | 0.000000000000000 0.000000000000000 0.000000000000000
548 | 0.000000000000000 0.000000000000000 0.000000000000000
549 | 2.598650290853717 1.974418135880447 1.702902980962828
550 | 2.609959552711677 1.963136867807416 1.705432642934205
551 | 2.632705705758182 1.933612652125469 1.713373245385777
552 | 2.638353454110483 1.916805181204380 1.715492444438587
553 | 2.648560512868702 1.908497187502477 1.720188861281226
554 | 0.000000000000000 0.000000000000000 0.000000000000000
555 | 2.656808229515103 1.896979790251672 1.722454813204745
556 | 2.663098891150673 1.885010850008924 1.726627691858731
557 | 2.668793187939025 1.871732556784089 1.729023066644731
558 | 0.000000000000000 0.000000000000000 0.000000000000000
559 | 0.000000000000000 0.000000000000000 0.000000000000000
560 | 2.672952617628329 1.859283969192339 1.731938952985192
561 | 2.674020545344389 1.843965752958401 1.735462511689779
562 | 2.679158821815816 1.832777812514111 1.738484907056514
563 | 2.683118929401992 1.804485441512012 1.744670571123077
564 | 2.683840038924091 1.789454409301619 1.748444432664072
565 | 2.683515144641440 1.777798561172258 1.751277105766900
566 | 2.681947329959744 1.763109190016442 1.753755549792850
567 | 2.681513033768933 1.747025424598958 1.757543487200159
568 | 0.000000000000000 0.000000000000000 0.000000000000000
569 | 0.000000000000000 0.000000000000000 0.000000000000000
570 | 2.678101984591996 1.706486467948342 1.768077262755206
571 | 0.000000000000000 0.000000000000000 0.000000000000000
572 | 0.000000000000000 0.000000000000000 0.000000000000000
573 | 2.670759541700890 1.676409424658678 1.776287199966322
574 | 2.667239283671984 1.662795202892150 1.779176695142559
575 | 2.662882420350069 1.647325449380554 1.782545113737048
576 | 2.660309203894053 1.634479843988063 1.786265879658322
577 | 0.000000000000000 0.000000000000000 0.000000000000000
578 | 0.000000000000000 0.000000000000000 0.000000000000000
579 | 2.652114606437667 1.624003920143609 1.788824809777964
580 | 2.642673091042870 1.608810201273186 1.792093224404026
581 | 0.000000000000000 0.000000000000000 0.000000000000000
582 | 0.000000000000000 0.000000000000000 0.000000000000000
583 | 2.628547206344404 1.580835063462755 1.796287471044820
584 | 2.618643106726310 1.567148601761938 1.797820602117819
585 | 2.610639476771663 1.552799474323271 1.799259045849004
586 | 0.000000000000000 0.000000000000000 0.000000000000000
587 | 0.000000000000000 0.000000000000000 0.000000000000000
588 | 2.574648054207344 1.509163747503568 1.801395791116891
589 | 2.560571193643641 1.498180144081367 1.802758337566964
590 | 2.550040707422590 1.483225684554452 1.802653950615501
591 | 0.000000000000000 0.000000000000000 0.000000000000000
592 | 0.000000000000000 0.000000000000000 0.000000000000000
593 | 0.000000000000000 0.000000000000000 0.000000000000000
594 | 2.509536759366971 1.445710056714134 1.805121363844624
595 | 2.493511636204430 1.424484022189151 1.803897579313871
596 | 2.479775788279167 1.415770701182203 1.804352387882331
597 | 2.463340237175442 1.399491892523187 1.803410756883719
598 | 2.453137663669873 1.387582354561427 1.803813357388270
599 | 2.418682802484935 1.362473394119572 1.801378297830257
600 | 2.400781690995838 1.350085219059680 1.800130317199373
601 | 2.366837584854955 1.325923892063723 1.798188305595546
602 | 2.352197871412833 1.311237262791282 1.796098577400800
603 | 2.336920918076650 1.299561505208130 1.794024905000595
604 | 2.320969383561003 1.287001474378347 1.793429429171664
605 | 0.000000000000000 0.000000000000000 0.000000000000000
606 | 0.000000000000000 0.000000000000000 0.000000000000000
607 | 0.000000000000000 0.000000000000000 0.000000000000000
608 | 2.289045355920346 1.265291316212612 1.788564276805358
609 | 2.272737205909763 1.250170354987016 1.786895518587780
610 | 2.256095185436544 1.240459255385343 1.783621494634094
611 | 0.000000000000000 0.000000000000000 0.000000000000000
612 | 0.000000000000000 0.000000000000000 0.000000000000000
613 | 2.240654521136100 1.228813099311784 1.781610952480672
614 | 2.225097876120878 1.216519974408995 1.779476559206610
615 | 2.206428847610200 1.203902113666099 1.775696087915382
616 | 2.189757355792404 1.191399468359612 1.773015511760606
617 | 2.168945200765181 1.182322427766400 1.769778106028988
618 | 2.153700099576417 1.169590133007279 1.766103873526645
619 | 2.142258655387297 1.162236991674648 1.763155438803669
620 | 2.124872266009437 1.149830400290597 1.758822697588951
621 | 2.108708602774984 1.138114097859319 1.755006634978516
622 | 2.074958971712529 1.116923191738103 1.745858448931610
623 | 2.059040810019408 1.117138609374950 1.743584139319916
624 | 2.041386711543073 1.093007469545887 1.736053187885439
625 | 2.022532958005273 1.082113004945178 1.730278555078160
626 | 1.990827146401217 1.062102938933762 1.721818120070041
627 | 1.982247686234337 1.053802089408198 1.716027154812228
628 | 1.969408644047543 1.046724168782693 1.711128362954532
629 | 1.955561533708146 1.035692325708151 1.706343884976167
630 | 1.946666835092760 1.028775747628116 1.701247634299101
631 | 1.932750146761185 1.018773188676136 1.695817447031519
632 | 0.000000000000000 0.000000000000000 0.000000000000000
633 | 1.909402577726839 1.003751787873183 1.684560403564981
634 | 0.000000000000000 0.000000000000000 0.000000000000000
635 | 1.901241063685511 0.995982445886467 1.678280374472073
636 | 1.887302934540498 0.987736414813472 1.672690803451193
637 | 0.000000000000000 0.000000000000000 0.000000000000000
638 | 0.000000000000000 0.000000000000000 0.000000000000000
639 | 0.000000000000000 0.000000000000000 0.000000000000000
640 | 0.000000000000000 0.000000000000000 0.000000000000000
641 | 1.879053086583652 0.981542826264889 1.667988941838186
642 | 1.866439568576803 0.971411923760331 1.660259268950438
643 | 0.000000000000000 0.000000000000000 0.000000000000000
644 | 1.855723816618261 0.963449669395411 1.655392461036391
645 | 1.845797485279876 0.957050070500862 1.650619437824992
646 | 1.835633543417112 0.948422624073433 1.643560193440283
647 | 0.000000000000000 0.000000000000000 0.000000000000000
648 | 0.000000000000000 0.000000000000000 0.000000000000000
649 | 0.000000000000000 0.000000000000000 0.000000000000000
650 | 1.773581590049312 0.915070384761490 1.609642819607600
651 | 0.000000000000000 0.000000000000000 0.000000000000000
652 | 0.000000000000000 0.000000000000000 0.000000000000000
653 | 0.000000000000000 0.000000000000000 0.000000000000000
654 | 0.000000000000000 0.000000000000000 0.000000000000000
655 | 1.763046253292037 0.911838582130474 1.603822413858131
656 | 0.000000000000000 0.000000000000000 0.000000000000000
657 | 0.000000000000000 0.000000000000000 0.000000000000000
658 | 1.749602698839120 0.905835976672953 1.597934931836580
659 | 1.739099327788435 0.900444657260032 1.592936101854312
660 | 0.000000000000000 0.000000000000000 0.000000000000000
661 | 1.729346363914398 0.895309350214342 1.585591971614837
662 | 0.000000000000000 0.000000000000000 0.000000000000000
663 | 0.000000000000000 0.000000000000000 0.000000000000000
664 | 1.719283254693597 0.890166924750073 1.579332862413216
665 | 1.707819933273657 0.887389349555477 1.573930112184480
666 | 1.696016112164240 0.882529556636180 1.569049259204232
667 | 1.649965795024367 0.865522214664594 1.541572249979025
668 | 1.627651586742802 0.859001901728104 1.530075639270682
669 | 1.603631857683659 0.852102714673052 1.519845708783051
670 | 1.587171471827010 0.847396907923588 1.512670185120368
671 | 1.574572283013558 0.846528611126593 1.507108335751186
672 | 0.000000000000000 0.000000000000000 0.000000000000000
673 | 0.000000000000000 0.000000000000000 0.000000000000000
674 | 1.551131361699490 0.841668041216276 1.495471863309143
675 | 1.537609095267388 0.839132509141438 1.490127704621519
676 | 1.522682797353882 0.836667883316526 1.484984403403191
677 | 1.508610079660047 0.833681550645782 1.479471503237217
678 | 1.477149125863787 0.827479165883732 1.469657569017629
679 | 1.461227328714946 0.826193410341095 1.464960102191005
680 | 1.446888504524918 0.824467313364340 1.459564985701221
681 | 1.413308945484260 0.820711188980102 1.449713302883014
682 | 1.393453653674750 0.818019980759556 1.445128829558277
683 | 1.350687318109705 0.814777403090603 1.435102045314774
684 | 0.000000000000000 0.000000000000000 0.000000000000000
685 | 1.306885208015158 0.814966311466317 1.424379042152905
686 | 1.287131704486168 0.814415345109580 1.419752967428851
687 | 1.266574180100523 0.814362631149517 1.414816195887726
688 | 1.245902757643107 0.816293788782438 1.410165803688850
689 | 0.000000000000000 0.000000000000000 0.000000000000000
690 | 0.000000000000000 0.000000000000000 0.000000000000000
691 | 1.203400349866200 0.818448448783896 1.400766651506099
692 | 1.179736762233124 0.820787217230939 1.395333815565517
693 | 1.157607670284978 0.822968941837078 1.390609028516331
694 | 1.133160247721843 0.823968468161465 1.386507943686816
695 | 1.112093443947078 0.827779483318618 1.382093430956965
696 | 0.000000000000000 0.000000000000000 0.000000000000000
697 | 1.088093289164902 0.832033145628500 1.376767742478126
698 | 1.065460821196450 0.835437504591042 1.371953421719923
699 | 1.039362481539245 0.839025613040628 1.367168634585020
700 | 1.017581447232397 0.844229606883968 1.362779700693176
701 | 0.994607650725673 0.847857800296846 1.359045063808380
702 | 0.000000000000000 0.000000000000000 0.000000000000000
703 | 0.000000000000000 0.000000000000000 0.000000000000000
704 | 0.945366119079531 0.856660416598017 1.349376089745019
705 | 0.923386327017438 0.865093716015588 1.344214660266305
706 | 0.000000000000000 0.000000000000000 0.000000000000000
707 | 0.874506116856394 0.868792072401727 1.336260391525496
708 | 0.843913421348879 0.877158637787911 1.331278066969664
709 | 0.792270020974099 0.884453686403987 1.323294282667398
710 | 0.759262488841752 0.889247309897130 1.318711258805139
711 | 0.732517166241143 0.892987331006048 1.314213751299560
712 | 0.711421347907120 0.903966143314656 1.309746077157744
713 | 0.687851078334849 0.907722533162937 1.307033923933209
714 | 0.664064061813460 0.910729724006425 1.304908790678346
715 | 0.639452705432733 0.916109096390553 1.301141497723485
716 | 0.615161608427610 0.920921502170630 1.297673964887142
717 | 0.564912119261078 0.923259912513080 1.293395389802478
718 | 0.542789810648918 0.927426817749140 1.290050472012700
719 | 0.518613048963453 0.931831394108599 1.286173520074691
720 | 0.496067973610811 0.936199473208750 1.283373627572202
721 | 0.451648583300840 0.942830967777601 1.277604525918271
722 | 0.427800387492152 0.945651433417808 1.275185256096726
723 | 0.403979775977497 0.948176564473176 1.272517095700056
724 | 0.380909696012786 0.953134854589649 1.269276126856721
725 | 0.358126153076964 0.956748355649257 1.267459789157451
726 | 0.335222500972682 0.960123937447900 1.264847213436061
727 | 0.314742463917712 0.964037101579785 1.262829369998441
728 | 0.291155337425327 0.966615694189491 1.261051102102047
729 | 0.271297169525406 0.971433611349953 1.258113665496137
730 | 0.218251502674014 0.970751345075581 1.253576769222131
731 | 0.000000000000000 0.000000000000000 0.000000000000000
732 | 0.197529939382409 0.974995040809271 1.250495114638909
733 | 0.000000000000000 0.000000000000000 0.000000000000000
734 | 0.173998919688527 0.977754966380663 1.247912154049485
735 | 0.000000000000000 0.000000000000000 0.000000000000000
736 | 0.000000000000000 0.000000000000000 0.000000000000000
737 | 0.157533624374989 0.986807636677694 1.245742717680596
738 | 0.135207737069326 0.992026659613218 1.242095996050070
739 | 0.115439275872558 0.994939613124551 1.240260354038919
740 | 0.092606178511325 0.999745321918008 1.238210518036106
741 | 0.063791333683145 1.001330195481763 1.236067275991687
742 | 0.043883361646593 1.005372348435865 1.233353349068561
743 | 0.026246847903150 1.004179460311608 1.232225006686979
744 | 0.007985165259294 1.009047339727104 1.229686366084566
745 | -0.004156925169463 1.014942296044178 1.226446467570736
746 | -0.018970410175084 1.020672358544333 1.223606314784481
747 | 0.000000000000000 0.000000000000000 0.000000000000000
748 | 0.000000000000000 0.000000000000000 0.000000000000000
749 | -0.020263405275807 1.041920868630526 1.220057508014386
750 | 0.000000000000000 0.000000000000000 0.000000000000000
751 | 0.000000000000000 0.000000000000000 0.000000000000000
752 | -0.071810507814133 1.060132662370180 1.207273654722160
753 | -0.091940622686687 1.071736718714724 1.200923990702224
754 | 0.000000000000000 0.000000000000000 0.000000000000000
755 | 0.000000000000000 0.000000000000000 0.000000000000000
756 | -0.057125179136924 1.066935967907217 1.210877838687000
757 | -0.068483583776110 1.075612996462567 1.206101015313434
758 | 0.000000000000000 0.000000000000000 0.000000000000000
759 | -0.071225309292951 1.086426356369898 1.201568059962425
760 | 0.000000000000000 0.000000000000000 0.000000000000000
761 | 0.000000000000000 0.000000000000000 0.000000000000000
762 | 0.000000000000000 0.000000000000000 0.000000000000000
763 | 


--------------------------------------------------------------------------------
/Data/quadData.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gitfredy/KalmanPort/7e5af03d02de7a2113737e9d8611a67b5584a43d/Data/quadData.mat


--------------------------------------------------------------------------------
/MatlabFiles/MatlabEquations/14GEqn.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gitfredy/KalmanPort/7e5af03d02de7a2113737e9d8611a67b5584a43d/MatlabFiles/MatlabEquations/14GEqn.mat


--------------------------------------------------------------------------------
/MatlabFiles/MatlabEquations/14GFunc.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gitfredy/KalmanPort/7e5af03d02de7a2113737e9d8611a67b5584a43d/MatlabFiles/MatlabEquations/14GFunc.mat


--------------------------------------------------------------------------------
/MatlabFiles/MatlabEquations/14LFunc.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gitfredy/KalmanPort/7e5af03d02de7a2113737e9d8611a67b5584a43d/MatlabFiles/MatlabEquations/14LFunc.mat


--------------------------------------------------------------------------------
/MatlabFiles/MatlabEquations/imuGEqn.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gitfredy/KalmanPort/7e5af03d02de7a2113737e9d8611a67b5584a43d/MatlabFiles/MatlabEquations/imuGEqn.mat


--------------------------------------------------------------------------------
/MatlabFiles/MatlabEquations/imuGFunc.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gitfredy/KalmanPort/7e5af03d02de7a2113737e9d8611a67b5584a43d/MatlabFiles/MatlabEquations/imuGFunc.mat


--------------------------------------------------------------------------------
/MatlabFiles/MatlabEquations/imuLFunc.mat:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gitfredy/KalmanPort/7e5af03d02de7a2113737e9d8611a67b5584a43d/MatlabFiles/MatlabEquations/imuLFunc.mat


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/RPYToRotMat.m:
--------------------------------------------------------------------------------
 1 | function [rotMat] = RPYToRotMat(rpy)
 2 | %RPY to Rotation Matrix about ZXY.
 3 | 
 4 | phi = rpy(1);
 5 | theta = rpy(2);
 6 | psi = rpy(3);
 7 | 
 8 | rotMat = [ cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta), cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta), -cos(phi)*sin(theta); ...
 9 |     -cos(phi)*sin(psi), cos(phi)*cos(psi), sin(phi);...
10 |     cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi), sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi), cos(phi)*cos(theta)];
11 | 
12 | 
13 | 
14 | 


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/RotMatToRPY.m:
--------------------------------------------------------------------------------
 1 | function [rpy] = RotMatToRPY(R)
 2 | %Rotation Matrix R (about ZXY)
 3 | 
 4 | 
 5 | phi = asin(R(2,3));
 6 | theta = atan3(-R(1,3)/cos(phi),R(3,3)/cos(phi));
 7 | psi = atan3(-R(2,1)/cos(phi),R(2,2)/cos(phi));
 8 | rpy =[phi;theta;psi];
 9 | 
10 | 
11 | 


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/atan3.m:
--------------------------------------------------------------------------------
1 | function theta = atan3(y, x)
2 | % atan2 that accepts symbolic expression
3 | 
4 | theta = 2*atan(y / ( sqrt(x^2+y^2) + x ));
5 | 
6 | end


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/calculateCovEquation.m:
--------------------------------------------------------------------------------
 1 | function [ covEqCalc ] = calculateCovEquation( A, B )
 2 | %CALCULATECOVEQUATION Summary of this function goes here
 3 | %   For use with Eq 70 of Kraft Paper
 4 | %   Computed as the (weighted/averaged) outer product of the transformed
 5 | %   points, vectors in input matrices.
 6 | 
 7 | 
 8 | mat1 = A(:,1)*B(:,1)';
 9 | mat2 = A(:,2)*B(:,2)';
10 | mat3 = A(:,3)*B(:,3)';
11 | mat4 = A(:,4)*B(:,4)';
12 | mat5 = A(:,5)*B(:,5)';
13 | mat6 = A(:,6)*B(:,6)';
14 | mat7 = A(:,7)*B(:,7)';
15 | mat8 = A(:,8)*B(:,8)';
16 | mat9 = A(:,9)*B(:,9)';
17 | mat10 = A(:,10)*B(:,10)';
18 | mat11 = A(:,11)*B(:,11)';
19 | mat12 = A(:,12)*B(:,12)';
20 | mat13 = A(:,13)*B(:,13)';
21 | mat14 = A(:,14)*B(:,14)';
22 | mat15 = A(:,15)*B(:,15)';
23 | mat16 = A(:,16)*B(:,16)';
24 | mat17 = A(:,17)*B(:,17)';
25 | mat18 = A(:,18)*B(:,18)';
26 | mat19 = A(:,19)*B(:,19)';
27 | mat20 = A(:,20)*B(:,20)';
28 | mat21 = A(:,21)*B(:,21)';
29 | mat22 = A(:,22)*B(:,22)';
30 | mat23 = A(:,23)*B(:,23)';
31 | mat24 = A(:,24)*B(:,24)';
32 | mat25 = A(:,25)*B(:,25)';
33 | mat26 = A(:,26)*B(:,26)';
34 | mat27 = A(:,27)*B(:,27)';
35 | mat28 = A(:,28)*B(:,28)';
36 | 
37 | %disp(mat28);
38 | 
39 | %covEqCalc = (mat1 + mat2 + mat3 + mat4 + mat5 + mat6 + mat7 + mat8 + mat9 + mat10 + mat11 + mat12 + ...
40 | %mat13 + mat14 + mat15 + mat16 + mat17 + mat18 + mat19 + mat20 + mat21 + mat22 + mat23 + mat24)/24; %12 for orient track Kraft Paper eq.68
41 | 
42 | covEqCalc = (mat1 + mat2 + mat3 + mat4 + mat5 + mat6 + mat7 + mat8 + mat9 + mat10 + mat11 + mat12 + ...
43 | mat13 + mat14 + mat15 + mat16 + mat17 + mat18 + mat19 + mat20 + mat21 + mat22 + mat23 + mat24 + mat25 + mat26 + mat27 + mat28)/28; %12 for orient track Kraft Paper eq.68
44 | 
45 | %disp(covEqCalc);
46 | 
47 | end
48 | 
49 | 


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/final650.m:
--------------------------------------------------------------------------------
  1 | 
  2 | %% Fredy Monterroza
  3 | %ESE 650 Final Project: Quadrotor control using state estimation from
  4 | %an EKF/UKF using homography from 2D images and IMU sensor data. Possible 
  5 | %augmentation includes reinforcement learning of gains or a PD-type
  6 | %iterative learning control. Possible use, surveillance, deployed quadrotor
  7 | %can optimally adapt to their environment due to the fact that the patrol
  8 | %pattern is repeatable. I think the effect of having patrolling quadrotors
  9 | %is the same or similar to that produced by people leaving their TV on even
 10 | %though they aren't home. This is supposed to deter would-be intruders from
 11 | %breaking into a residence/establishment. I think this is more true now,
 12 | %when the populace still isn't too familiar with autonomous drones, most
 13 | %people would assume there is someone monitoring/controlling these drones
 14 | %directly. Just a thought.
 15 | %Note, since system is linearizable near hover, an EKF may be used.
 16 | %Here, the Extended Kalman Filter is used to form a state estimate.
 17 | %Uses nPointPose.m and Matlab's Jacobian computation.
 18 | 
 19 | 
 20 | %NOTE: 14 State EKF tracking 9 dim state vector + biases. final650.m
 21 | 
 22 | %% Global Vars
 23 | global tagWorldCoords;
 24 | global tagIDs;
 25 | global cam2RobotHomoStable;
 26 | global kMat;
 27 | 
 28 | %% Load Data: Should be in KalmanPort Directory
 29 | addpath(genpath('/home/fredy/Desktop/KalmanPort'));
 30 | 
 31 | load('/Data/quadData.mat'); %Quadrotor Data Set: Vicon + IMU/Images
 32 | load('MatlabEquations/14GEqn');
 33 | load('MatlabEquations/14GFunc');
 34 | load('MatlabEquations/14LFunc');
 35 | 
 36 | 
 37 | %Tag ID's (9x12 matrix)
 38 | tagIDs = [57401312644, 58383764297, 59366215950, 61331119256, 63296022562, 65260925868,  1453707397,  4401062356,  9313320621, 10295772274, 14225578886, 17172933845; ...
 39 |  18155385498, 19137837151, 21102740457, 22085192110, 24050095416, 27979902028, 28962353681, 33874611946, 34857063599, 35839515252, 37804418558, 42716676823; ...
 40 |  43699128476, 46646483435, 47628935088, 49593838394, 56470999965, 57453451618, 61383258230, 12312814554, 18207524472, 23119782737, 27049589349, 28032041002; ...
 41 |  29014492655, 29996944308, 37856557532, 42768815797, 46698622409, 50628429021, 56523138939, 58488042245, 61435397204, 67330107122,  6470243610, 10400050222; ...
 42 |  12364953528, 17277211793, 32013986588, 35943793200, 37908696506, 42820954771, 52645471301, 55592826260,  5539930931, 13399544155, 29118770603, 31083673909; ...
 43 |  36978383827, 37960835480, 38943287133, 44837997051, 46802900357, 49750255316, 49802394290, 57662007514,  5644208879, 20380983674, 21363435327, 27258145245; ...
 44 |  44942274999, 63608856406,  7661251159, 14538412730, 22398025954, 25345380913, 32222542484, 62678543727, 24415068234, 31292229805, 63713134354, 25449658861; ...
 45 |  33309272085, 51975853492, 62782821675, 15677281305, 45150830895, 20641678544, 21624130197, 36360904992, 45202969869, 62887099623,  6939494376,  9886849335; ...
 46 |  29535882395, 41325302231, 13868794921, 19763504839, 21728408145, 27623118063, 40447128526, 41429580179, 46341838444, 52236548362, 37551912541, 59165848907   ];
 47 | 
 48 | % Camera Matrix (zero-indexed): Units = Pixels "Calibration Matrix"
 49 | kMat = [312.554757, 0.00000000, 170.720170; ...
 50 |  0.00000000, 313.225581, 134.038406; ...
 51 |  0.00000000, 0.00000000, 1.00000000   ];
 52 | 
 53 | 
 54 | %Robot To Camera Transformation:
 55 | rotZ = [cos(-pi/4), -sin(-pi/4), 0; ...
 56 |     sin(-pi/4), cos(-pi/4), 0; ...
 57 |     0, 0, 1];
 58 | rotX = [1, 0, 0; ...
 59 |     0, cos(pi), -sin(pi); ...
 60 |     0, sin(pi), cos(pi)];
 61 | rotRobot = rotX*rotZ;
 62 | transRobot = [-.04; 0; -.03];
 63 | c2rHomo = [rotRobot; zeros(1, 3)];
 64 | tRHomo = [transRobot; 1];
 65 | cam2RobotHomoStable = [c2rHomo, tRHomo];
 66 |             
 67 |             
 68 | kMatInv = inv(kMat);
 69 | tagIDs = tagIDs'; %%12x9
 70 | 
 71 | %% Pre-Compute April Tag Corner Locations (12x9, 108 tags)
 72 | accumX = 0;
 73 | whiteSpaceX = 0;
 74 | 
 75 | tagWorldCoords = {}; %Initialize April Tag Mat
 76 | for j = 1:12
 77 |     whiteSpaceY = 0; %Reset White Space Y
 78 |     accumY = 0;
 79 |     for k = 1:9
 80 |         %disp(k);
 81 |         %disp(j);
 82 |         tagIndex = sub2ind(size(tagIDs), j, k);
 83 |         %tagIndex = find(
 84 |         %tagIndex = sub2ind(size(tagIDs), k, j);
 85 |         
 86 |         tagWorldCoords{tagIndex}.tlx = accumX + whiteSpaceX;
 87 |         tagWorldCoords{tagIndex}.trx = accumX + whiteSpaceX;
 88 |         
 89 |         tagWorldCoords{tagIndex}.blx = accumX + whiteSpaceX + .152;
 90 |         tagWorldCoords{tagIndex}.brx = accumX + whiteSpaceX + .152;
 91 |         
 92 |         tagWorldCoords{tagIndex}.tly = accumY + whiteSpaceY;
 93 |         tagWorldCoords{tagIndex}.bly = accumY + whiteSpaceY;
 94 |         
 95 |         tagWorldCoords{tagIndex}.try = accumY + whiteSpaceY + .152;
 96 |         tagWorldCoords{tagIndex}.bry = accumY + whiteSpaceY + .152;
 97 |         
 98 |         if k == 3 || k == 6 %This increment in whitespace happens after tag at col 3, col 6 has been calculated
 99 |             whiteSpaceY = whiteSpaceY + .178;
100 |         else
101 |             whiteSpaceY = whiteSpaceY + .152;
102 |         end
103 |         accumY = accumY + .152;
104 |     end
105 |     accumX = accumX + .152;
106 |     whiteSpaceX = whiteSpaceX + .152;
107 | end
108 | 
109 | 
110 | 
111 | %% Initialize Parameters
112 | 
113 | numSamples = numel(sensorLog);
114 | 
115 | 
116 | %% Show Video of Flight
117 | 
118 | %Not only do I not always have an image, but I don't always have detected
119 | %tags within that image that may have been received. (Contrast issues?)
120 | 
121 | 
122 | imageIndices = []; %Holds Sample Indices which actually provide an image.
123 | imageWithTagInd = [];
124 | for imSamp = 1:numSamples
125 |     image = sensorLog{imSamp}.img;
126 |     if ~isempty(image);
127 |         %Only a sample with an image will have a tag, so save that index.
128 |         if ~isempty(sensorLog{imSamp}.id)
129 |             imageWithTagInd = [imageWithTagInd; imSamp];
130 |         end
131 |         imageIndices = [imageIndices; imSamp];
132 |         %disp(imSamp); %About sample 300 when its on the box.
133 |         imshow(image);
134 |         pause(.03); % I think its 30Hz image aquisition.
135 |     end
136 | end
137 | 
138 | 
139 | %% Show Video of Images with Tags
140 | 
141 | for imWithTags = 1:numel(imageWithTagInd)
142 |     figure(1);
143 |     imshow(sensorLog{imageWithTagInd(imWithTags)}.img);
144 |     pause(.03);
145 | end
146 | 
147 | 
148 | %% Parse qdLog (Vicon Data)
149 | viconSamples = numel(qdLog);
150 | viconEuler = zeros(3,viconSamples); %Roll Pitch Yaw
151 | viconPos = zeros(3, viconSamples);
152 | viconTime = zeros(1,viconSamples);
153 | bodyLinVel = zeros(3,viconSamples);
154 | bodyAngVel = zeros(3,viconSamples);
155 | 
156 | 
157 | for viconSamp = 1:viconSamples
158 |     viconEuler(:,viconSamp) = qdLog{viconSamp}{1}.euler;
159 |     viconPos(:,viconSamp) = qdLog{viconSamp}{1}.pos;
160 |     viconTime(viconSamp) = qdTimeLog{viconSamp};
161 |     bodyLinVel(:,viconSamp) = qdLog{viconSamp}{1}.vel_body;
162 |     bodyAngVel(:,viconSamp) = qdLog{viconSamp}{1}.omega;
163 | end
164 | 
165 | 
166 | 
167 | %% Parse Quadrotor Sensor (IMU) Data: Note, since this is wirelessly, transmitted packets may be lost.
168 | sensorSamples = numel(sensorLog);
169 | sensorIMU = zeros(6, sensorSamples);
170 | sensorTS = zeros(1,sensorSamples);
171 | 
172 | %All this data is with respect to body, different sampling than images with
173 | %tags but the same as imageIndices.
174 | for sensorSamp = 1:sensorSamples
175 |     accel = sensorLog{sensorSamp}.accImu;
176 |     angVel = sensorLog{sensorSamp}.omegaImu;
177 |     if ~isempty(accel) %assume angvel, t also not epmpty if accel isn't...
178 |         sensorIMU(:,sensorSamp) = [accel; angVel];
179 |         sensorTS(sensorSamp) = sensorLog{sensorSamp}.t; %diff this.
180 |     end
181 | 
182 | end
183 | 
184 | %Valid Timestamps will be >0, they are initialized/declared as 0 above.
185 | actualIMUDataReceived = sensorIMU(:,find(sensorTS>0)); %1293 Samples
186 | actualIMUinds = find(sensorTS>0);
187 | 
188 | %Data Collected at 33Hz, blind for 3.2424 seconds at peak
189 | blindInds = diff(actualIMUinds); % blindInds(840) = 107 time steps blind
190 | 
191 | %IMU bias in accel/gyro aXb = mean(actualIMUDataReceived(3,1:126)); 
192 | aXb = 0.4109;
193 | aYb = 0.4024;
194 | aZb = 9.6343;
195 | gXb = -2.6667e-04;
196 | gYb = 6.6667e-05;
197 | gZb = -0.0024;
198 | 
199 | 
200 | %% Initial Estimate of R, Q: Process and Measurement Covariance respectively.
201 | %I think when these are diagonal matrices they represent noise of each
202 | %element ierrespective of the correlation/dependence of other parts. i.e.
203 | %error in x axis accelerometer vs y axis accelerometer. (Variance)
204 | 
205 | %Quadrotor is on box/cube/platform for samples 0-218.
206 | stableQuadTime = sensorTS(1:281);
207 | stableValidInds = find(stableQuadTime>0); %Indices of Valid IMU data.
208 | numStableDataSamples = numel(stableValidInds); %124
209 | 
210 | %{
211 | varAx = var(sensorIMU(1,stableValidInds));
212 | varAy = var(sensorIMU(2,stableValidInds));
213 | varAz = var(sensorIMU(3,stableValidInds));
214 | varGx = var(sensorIMU(4,stableValidInds));
215 | varGy = var(sensorIMU(5,stableValidInds));
216 | varGz = var(sensorIMU(6,stableValidInds));
217 | %}
218 | 
219 | varAx = var(actualIMUDataReceived(1,1:100));
220 | varAy = var(actualIMUDataReceived(2,1:100));
221 | varAz = var(actualIMUDataReceived(3,1:100));
222 | varGx = var(actualIMUDataReceived(4,1:100));
223 | varGy = var(actualIMUDataReceived(5,1:100));
224 | varGz = var(actualIMUDataReceived(6,1:100));
225 | 
226 | 
227 | ukfR = diag([varAx, varAy, varAz, varGx, varGy, varGz]); %Measurement Cov
228 | %ukfQ = diag([3.0466e-006, 5.4396e-006, 4.2754e-005, 1e-4, 1e-4, 1e-4]); %Process Cov
229 | ukfQ = diag([3.0466e-006, 5.4396e-006, 4.2754e-005, 0.0110, 0.0281, 6.3328e-04]); %Process Cov
230 | 
231 | 
232 | %Vicon Error Found according to var(bodyLinVel(1,1:281)) etc, same for
233 | %bodyAngVel, but that data had really noisy stuff, so...
234 | 
235 | %% Aril Tags nPoint Pose Alg:
236 | %L.Quan, Linear N-Point Camera Pose Determination, IEEE Trans. on Pattern Analysis and Machine Intelligence, vol. 21, no.7, July 1999
237 | 
238 | visionRobot = [];
239 | for j = 1:numSamples
240 |   
241 |   if ~isempty(sensorLog{j}.id)
242 |     [vrobotPos, vrobotOrient] = nPointPose(sensorLog{j});
243 | 
244 |     visionRobot = [visionRobot, [vrobotPos(1:3); vrobotOrient]];
245 |   end
246 | end
247 | 
248 | 
249 | figure;
250 | plot3(visionRobot(1,:), visionRobot(2,:), visionRobot(3,:));
251 | grid on;
252 | 
253 | 
254 | 
255 | 
256 | %% EKF(Accel Bias Tracked, 14 Dim)
257 | 
258 | 
259 | %Initialize EKF with first two frames
260 | [robotPos, robotOrient, robotToWorld] = nPointPose(sensorLog{522});
261 | robotPos = robotPos(1:3);
262 | 
263 | %First Control Signal preserved in prevU for use when don't have data.
264 | U = [sensorLog{522}.accImu(1); sensorLog{522}.accImu(2); sensorLog{522}.accImu(3); sensorLog{522}.omegaImu(1); ...
265 |         sensorLog{522}.omegaImu(2); sensorLog{522}.omegaImu(3)]; 
266 | 
267 | prevU = U; %update prevU whenever have an input signal.
268 | 
269 | 
270 | 
271 | initVel = robotToWorld * qdLog{522}{1}.vel_body; %The initial state should be in the world/mat frame.
272 | Xest = [robotPos; initVel; robotOrient; 0; 0; 0; 0; 0];
273 | 
274 | 
275 | 
276 | P = 0*eye(14); %diag([0 0 0 0 0 0 0 0 0 ]);   % State
277 | R = diag([0.05, 0.05, 0.09, 1e-3, 1e-3, 1e-3, 0.05, 0.05, 0.09]);  % Process
278 | Q = diag([0.001, 0.001, 0.008, 0.001, 0.029, 0.001]); % Measurement
279 | 
280 | %6x14
281 | H = [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; ...
282 |      0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; ...
283 |      0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; ...
284 |      0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0; ...
285 |      0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1; ...
286 |      0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0]; %Cherry Pick the state vector components
287 |      
288 | 
289 | stateVector = [];
290 | stateVector = [stateVector, Xest]; %Add First State, initialization.
291 | prevTime = qdTimeLog{522};
292 | normP = [];
293 | normP = [normP, norm(P)];
294 | 
295 | worldLinVel = [];
296 | worldLinV = robotToWorld *bodyLinVel(:,522);
297 | worldLinVel = [worldLinVel, worldLinV];
298 | 
299 | 
300 | worldAngVel = [];
301 | worldAngV = robotToWorld *bodyAngVel(:,522);
302 | worldAngVel = [worldAngVel, worldAngV];
303 | 
304 | h = figure;
305 | figure(h);
306 | hold all;
307 | 
308 | %nPPpos = [];
309 | for i = 523:(numSamples-230) %Vicon Samples and Sensor Samples are same #, but have to check if actually have a sensor packet...
310 |    
311 |     %disp(i);  
312 |    
313 |     deltaT = qdTimeLog{i} - prevTime;%timeDiff(i);
314 |     
315 |  
316 |     if (~isempty(sensorLog{i}.accImu)) %Assume omegaImu also not empty.
317 |         U = [sensorLog{i}.accImu(1); sensorLog{i}.accImu(2); sensorLog{i}.accImu(3); sensorLog{i}.omegaImu(1); ...
318 |         sensorLog{i}.omegaImu(2); sensorLog{i}.omegaImu(3)]; 
319 | 
320 |         prevU = U; %update prevU whenever have an input signal.
321 |         
322 |     else
323 |        U = prevU;
324 |        %U = [(Xest(4:6)-prevState(4:6))/deltaT; (Xest(7:9)-prevState(7:9))/deltaT]; %Based on Previous State * change in time: m/s /deltaT =m/s^2
325 |     end
326 |  
327 |     
328 |      %EKF Prediction
329 |      %State Vector
330 |      Xn(1) = Xest(1); % x
331 |      Xn(2) = Xest(2); % y
332 |      Xn(3) = Xest(3); % z
333 |      Xn(4) = Xest(4); % velocity x
334 |      Xn(5) = Xest(5); % velocity y
335 |      Xn(6) = Xest(6); % velocity z
336 |      Xn(7) = Xest(7); % roll
337 |      Xn(8) = Xest(8); % pitch
338 |      Xn(9) = Xest(9); % yaw
339 |      Xn(10) = Xest(10); %Accelerometer x bias
340 |      Xn(11) = Xest(11); %Accelerometer y bias
341 |      Xn(12) = Xest(12); %Accelerometer z bias
342 |      Xn(13) = Xest(13); %Camera phi bias
343 |      Xn(14) = Xest(14); %Camera theta bias
344 |  
345 |      %Control Input
346 |      Vn(1) = U(1); %The Control input modeled with noise, (already noisy, by nature).
347 |      Vn(2) = U(2); 
348 |      Vn(3) = U(3);
349 |      Vn(4) = U(4); 
350 |      Vn(5) = U(5); 
351 |      Vn(6) = U(6); 
352 |   
353 |      
354 |      %IMU Error in Accelerometer/Gyro
355 |      errVn(1:6) = 0; %0 Mean Noise for Process Update (Prediction)
356 |      errVn(7:9) = sqrt((diag([varAx, varAy, varAz]))) * randn(3,1); %Gaussian Random Walk Model of accelerometer time varying bias
357 |      
358 |      
359 |      %G takes X(7)-X(9) instead of X(4)-X(6) for IMU
360 |      G   = gFunc(Vn(1), Vn(2), Vn(3), Vn(4), Vn(5), Vn(6), Xn(10), Xn(11), Xn(12), Xn(7), Xn(8), Xn(9), ...
361 |      deltaT, errVn(1), errVn(2), errVn(3), errVn(4), errVn(5), errVn(6)); 
362 |      %L takes X(7-9) instead also, other remains same
363 |      L   = lFunc(Vn(4), Vn(5), Vn(6), Xn(7), Xn(8), Xn(9), deltaT, errVn(4), errVn(5), errVn(6)); %subs(LJ);
364 |      Xest = gEqn(Vn(1), Vn(2), Vn(3), Vn(4), Vn(5), Vn(6), Xn(1), Xn(10), Xn(11), Xn(12), Xn(13), Xn(14), Xn(2), Xn(3), Xn(4), Xn(5), Xn(6), Xn(7), Xn(8), Xn(9), ...
365 |      deltaT, errVn(1), errVn(2), errVn(3), errVn(4), errVn(5), errVn(6), errVn(7), errVn(8), errVn(9)); 
366 |      %G is 9x9, L should be 9x6, since R is 6x6;
367 |      P    = G * P * G' + L * R * L';
368 | 
369 |      
370 |   % EKF Correction: Only When have an image TAG to do nPointPose
371 |   if ~isempty(sensorLog{i}.id)
372 |       
373 |     [Zr Zo robotToWorld ] = nPointPose(sensorLog{i}); %nPointPose spits out [x, y, z, roll, pitch, yaw] in world/april tag mat frame  
374 |       
375 |     Z = [Zr(1:3); Zo];
376 |     %nPPpos = [nPPpos, Zr(1:3)];
377 |     
378 |     K = P * H' * inv(H * P * H' + Q); 
379 |     Xest = Xest + K * (Z - H*Xest); %Cherry Pick the parts of Xest
380 |     P    = (eye(14) - K * H) * P;
381 |  
382 |   end
383 |     
384 |   
385 |     worldLinV = robotToWorld *bodyLinVel(:,i);
386 |     worldLinVel = [worldLinVel, worldLinV];
387 | 
388 | 
389 |     worldAngV = robotToWorld *bodyAngVel(:,i);
390 |     worldAngVel = [worldAngVel, worldAngV];
391 | 
392 |   
393 |     stateVector = [stateVector, Xest];
394 |     
395 |     prevTime = qdTimeLog{i};
396 |     
397 |     %2-norm of a matrix => largest singuluar value => sq.rt(largest eigen value(P'P))
398 |     normP = [normP, norm(P)]; 
399 |     
400 | 
401 |     
402 |     %Show State Estimate and State Coveriance during Flight with Data Given
403 |     figure(h);
404 |     subplot(2,3, [1 4])
405 |     grid on
406 |     plot3(Xest(1), Xest(2), Xest(3));
407 |     hold on
408 |     subplot(2,3, [2 3])
409 |     %subplot(2,3, 3)
410 |     plot(i, norm(P));
411 |     hold on
412 |     subplot(2,3, [5 6])
413 |     %subplot(2,3, 6)
414 |     imshow(sensorLog{i}.img);
415 |     
416 |      
417 | end
418 | 
419 | 
420 | %% Plot Stuff
421 | 
422 | %{
423 | %Ground Truth Trajectory
424 | figure(1);
425 | plot3(viconPos(1,:), viconPos(2,:), viconPos(3,:));
426 | %Ground Truth Orientation
427 | figure(2);
428 | plot(viconEuler');
429 | figure;
430 | plot(bodyLinVel');
431 | figure;
432 | plot(bodyAngVel');
433 | 
434 | 
435 | %Data (Only Packets Not Dropped Shown!!)
436 | figure;
437 | plot(actualIMUDataReceived(1:3,:)');
438 | figure;
439 | plot(actualIMUDataReceived(4:6,:)'); %Gyro
440 | %}
441 | %{
442 | figure;
443 | plot3(nPPpos(1,:), nPPpos(2,:), nPPpos(3,:), 'r');
444 | grid on;
445 | %}
446 | 
447 | 
448 | 
449 | %PLOT ALL 3D
450 | figure;
451 | %plot3(stateVector(1,:), stateVector(2,:), stateVector(3,:), 'b');
452 | plot3(stateVector(1,1:1493), stateVector(2,1:1493), stateVector(3,1:1493), 'b');
453 | hold on
454 | plot3(stateVector(1,831:938), stateVector(2,831:938), stateVector(3,831:938), 'c-'); %Blind
455 | plot3(stateVector(1,1493:end), stateVector(2,1493:end), stateVector(3,1493:end), 'k'); %End of Tags
456 | plot3(visionRobot(1,:), visionRobot(2,:), visionRobot(3,:), 'r'); %Measurement (Blind Data Excluded, of course)
457 | plot3(viconPos(1,522:end)+ 1.5, viconPos(2,522:end)+1.2081, viconPos(3,522:end), 'g'); %Vicon Ground Truth
458 | grid on; 
459 | 
460 | 
461 | %EKF vs Vicon 3D
462 | figure;
463 | plot3(stateVector(1,1:1493), stateVector(2,1:1493), stateVector(3,1:1493), 'b');
464 | hold on
465 | plot3(stateVector(1,831:938), stateVector(2,831:938), stateVector(3,831:938), 'c-'); %Blind
466 | plot3(viconPos(1,522:2015)+ 1.5, viconPos(2,522:2015)+1.2081, viconPos(3,522:2015), 'g');
467 | grid on;
468 | 
469 | 
470 | %x
471 | figure;
472 | subplot(3,1,1);
473 | title('x')
474 | plot(stateVector(1,:), 'b');
475 | %figure;
476 | subplot(3,1,2);
477 | plot(visionRobot(1,:), 'r');
478 | %figure;
479 | subplot(3,1,3);
480 | plot(viconPos(1,522:end)+1.5, 'g'); %1.5 for Coordinate Frame Offset of Vicon. 
481 | 
482 | 
483 | %y
484 | figure;
485 | subplot(3,1,1);
486 | title('y')
487 | plot(stateVector(2,:), 'b');
488 | %figure;
489 | subplot(3,1,2);
490 | plot(visionRobot(2,:), 'r');
491 | %figure;
492 | subplot(3,1,3);
493 | plot(viconPos(2,522:end)+1.2081, 'g'); %1.2081 for Coordinate Frame Offset of Vicon. 
494 | 
495 | 
496 | %z
497 | figure;
498 | subplot(3,1,1);
499 | title('z')
500 | plot(stateVector(3,:), 'b');
501 | %figure;
502 | subplot(3,1,2);
503 | plot(visionRobot(3,:), 'r');
504 | %figure;
505 | subplot(3,1,3);
506 | plot(viconPos(3,522:end), 'g');
507 | 
508 | 
509 | %vx
510 | figure;
511 | subplot(2,1,1);
512 | title('vx')
513 | plot(stateVector(4,:), 'b');
514 | %figure;
515 | subplot(2,1,2);
516 | plot(worldLinVel(1,:), 'g');
517 | 
518 | 
519 | %vy
520 | figure;
521 | subplot(2,1,1);
522 | title('vy')
523 | plot(stateVector(5,:), 'b');
524 | %figure;
525 | subplot(2,1,2);
526 | plot(worldLinVel(2,:), 'g');
527 | 
528 | 
529 | %vz
530 | figure;
531 | subplot(2,1,1);
532 | title('vz')
533 | plot(stateVector(6,:), 'b');
534 | %figure;
535 | subplot(2,1,2);
536 | plot(worldLinVel(3,:), 'g');
537 | 
538 | 
539 | 
540 | 
541 | %Angles Roll
542 | figure;
543 | subplot(3,1,1);
544 | title('roll')
545 | plot(stateVector(7,:), 'b');
546 | %figure;
547 | subplot(3,1,2);
548 | plot(visionRobot(4,:), 'r');
549 | %figure;
550 | subplot(3,1,3);
551 | plot(viconEuler(1,522:2084), 'g');
552 | 
553 | %Angles Pitch
554 | figure;
555 | subplot(3,1,1);
556 | title('pitch')
557 | plot(stateVector(8,:), 'b');
558 | %figure;
559 | subplot(3,1,2);
560 | plot(visionRobot(5,:), 'r');
561 | %figure;
562 | subplot(3,1,3);
563 | plot(viconEuler(2,522:2084), 'g');
564 | 
565 | 
566 | %Angles Yaw
567 | figure;
568 | subplot(3,1,1);
569 | title('yaw')
570 | plot(stateVector(9,:), 'b');
571 | %figure;
572 | subplot(3,1,2);
573 | plot(visionRobot(6,:), 'r');
574 | %figure;
575 | subplot(3,1,3);
576 | plot(viconEuler(3,522:2084), 'g');
577 | 
578 | 
579 | 
580 | %EKF and Covariance Evolution
581 | figure;
582 | subplot(2,1,1)
583 | grid on
584 | title('Position State Estimate')
585 | plot3(stateVector(1,:), stateVector(2,:), stateVector(3,:));
586 | hold on
587 | plot3(stateVector(1,831:938), stateVector(2,831:938), stateVector(3,831:938), 'c-'); %Blind
588 | hold on
589 | plot3(stateVector(1,1493:end), stateVector(2,1493:end), stateVector(3,1493:end), 'g-'); %No Observed Tags
590 | subplot(2,1, 2)
591 | title('Covariance Evolution')
592 | plot([522:2084], normP); %2314numel(normP)
593 | hold on
594 | plot([522+831:522+938], normP(831:938), 'c');
595 | hold on
596 | plot([522+1493:522+numel(normP)], normP(1493:end), 'g');
597 | 
598 | % Error
599 | derp = bsxfun(@plus, viconPos, [1.5; 1.2081; 0]); %Coorindate Frame Offset
600 | theError =  derp(1:3, 522:2084) - stateVector(1:3,:); %2084
601 | fprintf('Standard Mean Error:\n');
602 | disp(std(mean(theError)));
603 | 
604 | 
605 | 
606 | 


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/imuUKF.m:
--------------------------------------------------------------------------------
  1 | 
  2 | %% Fredy Monterroza
  3 | %Quadrotor control using state estimation from an EKF/UKF using homography 
  4 | %from 2D images and IMU sensor data. Possible 
  5 | %augmentation includes reinforcement learning of gains or a PD-type
  6 | %iterative learning control. Possible use, surveillance, deployed quadrotor
  7 | %can optimally adapt to their environment due to the fact that the patrol
  8 | %pattern is repeatable.
  9 | %Note, since system is linearizable near hover, an EKF may be used.
 10 | %Here, the Unscented Kalman Filter is used to provide the state estimate.
 11 | %Uses quadUKF.m, nPointPose.m
 12 | 
 13 | %% Global Vars
 14 | global tagWorldCoords;
 15 | global tagIDs;
 16 | global cam2RobotHomoStable;
 17 | global kMat;
 18 | 
 19 | %% Load Data 
 20 | 
 21 | addpath(genpath('/home/fredy/Desktop/KalmanPort'));
 22 | 
 23 | load('/Data/quadData.mat'); %Quadrotor Data Set: Vicon + IMU/Images
 24 | load('MatlabEquations/14GEqn');
 25 | 
 26 | %hEqn=@(x)[x(1); x(2); x(3); x(7); x(8); x(9)];
 27 | 
 28 | 
 29 | %Tag ID's (9x12 matrix)
 30 | tagIDs = [57401312644, 58383764297, 59366215950, 61331119256, 63296022562, 65260925868,  1453707397,  4401062356,  9313320621, 10295772274, 14225578886, 17172933845; ...
 31 |  18155385498, 19137837151, 21102740457, 22085192110, 24050095416, 27979902028, 28962353681, 33874611946, 34857063599, 35839515252, 37804418558, 42716676823; ...
 32 |  43699128476, 46646483435, 47628935088, 49593838394, 56470999965, 57453451618, 61383258230, 12312814554, 18207524472, 23119782737, 27049589349, 28032041002; ...
 33 |  29014492655, 29996944308, 37856557532, 42768815797, 46698622409, 50628429021, 56523138939, 58488042245, 61435397204, 67330107122,  6470243610, 10400050222; ...
 34 |  12364953528, 17277211793, 32013986588, 35943793200, 37908696506, 42820954771, 52645471301, 55592826260,  5539930931, 13399544155, 29118770603, 31083673909; ...
 35 |  36978383827, 37960835480, 38943287133, 44837997051, 46802900357, 49750255316, 49802394290, 57662007514,  5644208879, 20380983674, 21363435327, 27258145245; ...
 36 |  44942274999, 63608856406,  7661251159, 14538412730, 22398025954, 25345380913, 32222542484, 62678543727, 24415068234, 31292229805, 63713134354, 25449658861; ...
 37 |  33309272085, 51975853492, 62782821675, 15677281305, 45150830895, 20641678544, 21624130197, 36360904992, 45202969869, 62887099623,  6939494376,  9886849335; ...
 38 |  29535882395, 41325302231, 13868794921, 19763504839, 21728408145, 27623118063, 40447128526, 41429580179, 46341838444, 52236548362, 37551912541, 59165848907   ];
 39 | 
 40 | % Camera Matrix (zero-indexed): Units = Pixels "Calibration Matrix"
 41 | kMat = [312.554757, 0.00000000, 170.720170; ...
 42 |  0.00000000, 313.225581, 134.038406; ...
 43 |  0.00000000, 0.00000000, 1.00000000   ];
 44 | 
 45 | 
 46 | %Robot To Camera Transformation:
 47 | rotZ = [cos(-pi/4), -sin(-pi/4), 0; ...
 48 |     sin(-pi/4), cos(-pi/4), 0; ...
 49 |     0, 0, 1];
 50 | rotX = [1, 0, 0; ...
 51 |     0, cos(pi), -sin(pi); ...
 52 |     0, sin(pi), cos(pi)];
 53 | rotRobot = rotX*rotZ;
 54 | transRobot = [-.04; 0; -.03];
 55 | c2rHomo = [rotRobot; zeros(1, 3)];
 56 | tRHomo = [transRobot; 1];
 57 | cam2RobotHomoStable = [c2rHomo, tRHomo];
 58 |             
 59 |             
 60 | kMatInv = inv(kMat);
 61 | tagIDs = tagIDs'; %%12x9
 62 | 
 63 | %% Pre-Compute April Tag Corner Locations (12x9, 108 tags)
 64 | accumX = 0;
 65 | whiteSpaceX = 0;
 66 | 
 67 | tagWorldCoords = {}; %Initialize April Tag Mat
 68 | for j = 1:12
 69 |     whiteSpaceY = 0; %Reset White Space Y
 70 |     accumY = 0;
 71 |     for k = 1:9
 72 |         %disp(k);
 73 |         %disp(j);
 74 |         tagIndex = sub2ind(size(tagIDs), j, k);
 75 |         %tagIndex = find(
 76 |         %tagIndex = sub2ind(size(tagIDs), k, j);
 77 |         
 78 |         tagWorldCoords{tagIndex}.tlx = accumX + whiteSpaceX;
 79 |         tagWorldCoords{tagIndex}.trx = accumX + whiteSpaceX;
 80 |         
 81 |         tagWorldCoords{tagIndex}.blx = accumX + whiteSpaceX + .152;
 82 |         tagWorldCoords{tagIndex}.brx = accumX + whiteSpaceX + .152;
 83 |         
 84 |         tagWorldCoords{tagIndex}.tly = accumY + whiteSpaceY;
 85 |         tagWorldCoords{tagIndex}.bly = accumY + whiteSpaceY;
 86 |         
 87 |         tagWorldCoords{tagIndex}.try = accumY + whiteSpaceY + .152;
 88 |         tagWorldCoords{tagIndex}.bry = accumY + whiteSpaceY + .152;
 89 |         
 90 |         if k == 3 || k == 6 %This increment in whitespace happens after tag at col 3, col 6 has been calculated
 91 |             whiteSpaceY = whiteSpaceY + .178;
 92 |         else
 93 |             whiteSpaceY = whiteSpaceY + .152;
 94 |         end
 95 |         accumY = accumY + .152;
 96 |     end
 97 |     accumX = accumX + .152;
 98 |     whiteSpaceX = whiteSpaceX + .152;
 99 | end
100 | 
101 | 
102 | %% April Tag Corners
103 | 
104 | %Bottom Right is now Origin
105 | %L.Quan, Linear N-Point Camera Pose Determination, IEEE Trans. on Pattern Analysis and Machine Intelligence, vol. 21, no.7, July 1999
106 | 
107 | %% Initialize Parameters
108 | 
109 | numSamples = numel(sensorLog);
110 | 
111 | 
112 | %% Show Video of Flight
113 | 
114 | %Not only do I not always have an image, but I don't always have detected
115 | %tags within that image that may have been received. (Contrast issues?)
116 | 
117 | imageIndices = []; %Holds Sample Indices which actually provide an image.
118 | imageWithTagInd = [];
119 | for imSamp = 1:numSamples
120 |     figure(1);
121 |     image = sensorLog{imSamp}.img;
122 |     if ~isempty(image);
123 |         %Only a sample with an image will have a tag, so save that index.
124 |         if ~isempty(sensorLog{imSamp}.id)
125 |             imageWithTagInd = [imageWithTagInd; imSamp];
126 |         end
127 |         imageIndices = [imageIndices; imSamp];
128 |         %disp(imSamp); %About sample 300 when its on the box.
129 |         imshow(image);
130 |         pause(.03); % I think its 20Hz image aquisition.
131 |     end
132 | end
133 | 
134 | 
135 | %% Show Video of Images with Tags
136 | %{
137 | for imWithTags = 1:numel(imageWithTagInd)
138 |     figure(1);
139 |     imshow(sensorLog{imageWithTagInd(imWithTags)}.img);
140 |     pause(.02);
141 | end
142 | %}
143 | 
144 | %% Parse qdLog (Vicon Data)
145 | viconSamples = numel(qdLog);
146 | viconEuler = zeros(3,viconSamples); %Roll Pitch Yaw
147 | viconPos = zeros(3, viconSamples);
148 | viconTime = zeros(1,viconSamples);
149 | bodyLinVel = zeros(3,viconSamples);
150 | bodyAngVel = zeros(3,viconSamples);
151 | 
152 | 
153 | for viconSamp = 1:viconSamples
154 |     %vEul = qdLog{viconSamp}{1}.euler;
155 |     %vPos = qdLog{viconSamp}{1}.pos;
156 |     viconEuler(:,viconSamp) = qdLog{viconSamp}{1}.euler;
157 |     viconPos(:,viconSamp) = qdLog{viconSamp}{1}.pos;
158 |     viconTime(viconSamp) = qdTimeLog{viconSamp};
159 |     bodyLinVel(:,viconSamp) = qdLog{viconSamp}{1}.vel_body;
160 |     bodyAngVel(:,viconSamp) = qdLog{viconSamp}{1}.omega;
161 | end
162 | 
163 | 
164 | 
165 | %% Parse Quadrotor Sensor (IMU) Data: Note, since this is wirelessly, transmitted packets may be lost.
166 | sensorSamples = numel(sensorLog);
167 | sensorIMU = zeros(6, sensorSamples);
168 | sensorTS = zeros(1,sensorSamples);
169 | 
170 | %All this data is with respect to body, different sampling than images with
171 | %tags but the same as imageIndices.
172 | for sensorSamp = 1:sensorSamples
173 |     accel = sensorLog{sensorSamp}.accImu;
174 |     angVel = sensorLog{sensorSamp}.omegaImu;
175 |     if ~isempty(accel) %assume angvel, t also not epmpty if accel isn't...
176 |         sensorIMU(:,sensorSamp) = [accel; angVel];
177 |         sensorTS(sensorSamp) = sensorLog{sensorSamp}.t; %diff this.  
178 |     end
179 | 
180 | end
181 | 
182 | %Valid Timestamps will be >0, they are initialized/declared as 0 above.
183 | actualIMUDataReceived = sensorIMU(:,find(sensorTS>0)); %1293 Samples
184 | actualIMUinds = find(sensorTS>0);
185 | 
186 | %Data Collected at 33Hz, blind for 3.2424 seconds at peak
187 | blindInds = diff(actualIMUinds); % blindInds(840) = 140 time steps blind
188 | 
189 | %IMU bias in accel/gyro aXb = mean(actualIMUDataReceived(3,1:126)); 
190 | aXb = 0.4109;
191 | aYb = 0.4024;
192 | aZb = 9.6343;
193 | gXb = -2.6667e-04;
194 | gYb = 6.6667e-05;
195 | gZb = -0.0024;
196 | 
197 | 
198 | %% Initial Estimate of R, Q: Process and Measurement Covariance respectively.
199 | %I think when these are diagonal matrices they represent noise of each
200 | %element ierrespective of the correlation/dependence of other parts. i.e.
201 | %error in x axis accelerometer vs y axis accelerometer. (Variance)
202 | 
203 | %Quadrotor is on box/cube/platform for samples 0-218.
204 | stableQuadTime = sensorTS(1:281);
205 | stableValidInds = find(stableQuadTime>0); %Indices of Valid IMU data.
206 | numStableDataSamples = numel(stableValidInds); %124
207 | 
208 | %{
209 | varAx = var(sensorIMU(1,stableValidInds));
210 | varAy = var(sensorIMU(2,stableValidInds));
211 | varAz = var(sensorIMU(3,stableValidInds));
212 | varGx = var(sensorIMU(4,stableValidInds));
213 | varGy = var(sensorIMU(5,stableValidInds));
214 | varGz = var(sensorIMU(6,stableValidInds));
215 | %}
216 | 
217 | varAx = var(actualIMUDataReceived(1,1:100));
218 | varAy = var(actualIMUDataReceived(2,1:100));
219 | varAz = var(actualIMUDataReceived(3,1:100));
220 | varGx = var(actualIMUDataReceived(4,1:100));
221 | varGy = var(actualIMUDataReceived(5,1:100));
222 | varGz = var(actualIMUDataReceived(6,1:100));
223 | 
224 | 
225 | ukfR = diag([varAx, varAy, varAz, varGx, varGy, varGz]); %Measurement Cov
226 | ukfQ = diag([3.0466e-006, 5.4396e-006, 4.2754e-005, 0.0110, 0.0281, 6.3328e-04]); %Process Cov
227 | 
228 | 
229 | %% Aril Tags nPoint Pose Alg:
230 | 
231 | visionRobot = [];
232 | for j = 1:numSamples
233 |   
234 |   if ~isempty(sensorLog{j}.id)
235 |     [vrobotPos, vrobotOrient] = nPointPose(sensorLog{j});
236 | 
237 |     visionRobot = [visionRobot, [vrobotPos(1:3); vrobotOrient]];
238 |   end
239 | end
240 | 
241 | %figure;
242 | plot3(visionRobot(1,:), visionRobot(2,:), visionRobot(3,:));
243 | grid on;
244 | 
245 | 
246 | 
247 | %% UKF
248 | 
249 | 
250 | %Initialize EKF with first two frames
251 | [robotPos, robotOrient, robotToWorld] = nPointPose(sensorLog{522});
252 | robotPos = robotPos(1:3);
253 | 
254 | processInput = [];
255 | poseNPP = [];
256 | poseNPP = [poseNPP, [robotPos; robotOrient]];
257 | 
258 | %First Control Signal preserved in prevU for use when don't have data.
259 | U = [sensorLog{522}.accImu(1); sensorLog{522}.accImu(2); sensorLog{522}.accImu(3); sensorLog{522}.omegaImu(1); ...
260 |         sensorLog{522}.omegaImu(2); sensorLog{522}.omegaImu(3)]; 
261 | 
262 | processInput = [processInput, U];    
263 |         
264 | prevU = U;
265 | 
266 | % Estimated state: Initialization! 
267 | initVel = robotToWorld * qdLog{522}{1}.vel_body;
268 | Xest = [robotPos; initVel; robotOrient; 0; 0; 0; 0; 0];
269 | 
270 | P = .01*eye(14);  % State Covariance
271 |    
272 | stateVector = [];
273 | stateVector = [stateVector, Xest]; %Add First State, initialization.
274 | prevTime = qdTimeLog{522};
275 | normP = [];
276 | normP = [normP, norm(P)];
277 | 
278 | quadTimeLog = [];
279 | quadTimeLog = [quadTimeLog, qdTimeLog{522}];
280 | 
281 | h = figure;
282 | figure(h);
283 | hold all;
284 | 
285 | global herp;
286 | herp = [];
287 | for i = 523:(numSamples-230) %Vicon Samples and Sensor Samples are same #, but have to check if actually have a sensor packet...
288 |     disp(i);
289 |     if (i==1284) %Stop UKF at 'Blind' Stage, covariance diverges rapidly.
290 |            break;
291 |            
292 |     else
293 |     
294 |     deltaT = qdTimeLog{i} - prevTime;
295 |    
296 |     if (~isempty(sensorLog{i}.accImu)) %Assume omegaImu also not empty.
297 |         U = [sensorLog{i}.accImu(1); sensorLog{i}.accImu(2); sensorLog{i}.accImu(3); sensorLog{i}.omegaImu(1); ...
298 |         sensorLog{i}.omegaImu(2); sensorLog{i}.omegaImu(3)]; 
299 |         prevU = U;
300 |     else
301 |        %If we don't have a sensor packet, assume 0 linear/angular accel
302 |        %If base it off previous acceleration risk propogating noise if it was noisy
303 |        U = prevU;
304 |        %U = [(Xest(4:6)-prevState(4:6))/deltaT; (Xest(7:9)-prevState(7:9))/deltaT]; %Based on Previous State * change in time: m/s /deltaT =m/s^2
305 |     end
306 |     
307 |     processInput = [processInput, U]; 
308 | 
309 |      %Control Input
310 |      Vn(1) = U(1); %The Control input modeled with noise, (already noisy, by nature).
311 |      Vn(2) = U(2);
312 |      Vn(3) = U(3);
313 |      Vn(4) = U(4); 
314 |      Vn(5) = U(5); 
315 |      Vn(6) = U(6);
316 |      
317 |      %IMU Error in Accelerometer/Gyro
318 |      errVn(1:6) = 0; %0 Mean Noise for Process Update (Prediction)
319 |      errVn(7:9) = sqrt((diag([varAx, varAy, varAz]))) * randn(3,1); %Gaussian Random Walk Model of accelerometer time varying bias
320 |      
321 |      %herp = [herp, deltaT];
322 |   % UKF Correction: Only When have an image TAG to do nPointPose
323 |   if ~isempty(sensorLog{i}.id)
324 |       
325 |     [Zr Zo robotToWorld ] = nPointPose(sensorLog{i}); %nPointPose spits out [x, y, z, roll, pitch, yaw] in world/april tag mat frame  
326 |       
327 |     Z = [Zr(1:3); Zo];
328 |     %UKF
329 |     [Xest P] = quadUKF(Xest, P, deltaT, Z, gEqn, errVn, Vn);
330 |     
331 |     poseNPP = [poseNPP, Z];
332 |   else
333 |       
334 |     Z = [];
335 |     [Xest P] = quadUKF(Xest, P, deltaT, Z, gEqn, errVn, Vn);
336 |    
337 |     poseNPP = [poseNPP, [0; 0; 0; 0; 0; 0]]; %All 0 entry indicates poseNPP did not have data to compute measurement update pose.
338 |   end
339 | 
340 |     %Angular Velocity of Quadrotor:
341 |     %pqr = [cos(rpy(2)), 0, -cos(rpy(1))*sin(rpy(2)); 0, 1, sin(rpy(1)); sin(rpy(2)), 0, cos(rpy(1))*cos(rpy(2))]*data{angVels}.drpy;
342 |     
343 |     
344 |     stateVector = [stateVector, Xest];
345 |     
346 |     prevTime = qdTimeLog{i};
347 |     
348 |     normP = [normP, norm(P)];
349 |     
350 |     quadTimeLog = [quadTimeLog, qdTimeLog{i}];
351 |     
352 |     
353 |     figure(h);
354 |     subplot(2,3, [1 4])
355 |     grid on
356 |     plot3(Xest(1), Xest(2), Xest(3));
357 |     hold on
358 |     subplot(2,3, [2 3])
359 |     plot(i, norm(P));
360 |     hold on
361 |     subplot(2,3, [5 6])
362 |     imshow(sensorLog{i}.img);
363 |     
364 |     end %close i=1284
365 |     
366 | end
367 | 
368 | 
369 | %% Plot Stuff
370 | 
371 | %{
372 | %Ground Truth Trajectory
373 | figure(1);
374 | plot3(viconPos(1,:), viconPos(2,:), viconPos(3,:));
375 | %Ground Truth Orientation
376 | figure(2);
377 | plot(viconEuler');
378 | figure;
379 | plot(bodyLinVel');
380 | figure;
381 | plot(bodyAngVel');
382 | 
383 | 
384 | %Data (Only Packets Not Dropped Shown!!)
385 | figure;
386 | plot(actualIMUDataReceived(1:3,:)');
387 | figure;
388 | plot(actualIMUDataReceived(4:6,:)'); %Gyro
389 | %}
390 | 
391 | 
392 | %UKF vs Vicon
393 | figure;
394 | plot3(stateVector(1,:), stateVector(2,:), stateVector(3,:), 'b');
395 | hold on
396 | plot3(viconPos(1,522:2015)+ 1.5, viconPos(2,522:2015)+1.2081, viconPos(3,522:2015), 'g');
397 | grid on;
398 | 
399 | %UKF vs NPP
400 | figure;
401 | plot3(stateVector(1,:), stateVector(2,:), stateVector(3,:), 'b');
402 | hold on
403 | plot3(stateVector(1,3), stateVector(2,3), stateVector(3,3),  '-X'); %Blind
404 | %plot3(viconPos(1,522:2015)+ 1.5, viconPos(2,522:2015)+1.2081, viconPos(3,522:2015), 'g');
405 | %plot3(viconPos(1,522:1368)+ 1.5, viconPos(2,522:1368)+1.2081, viconPos(3,522:1368), 'g');
406 | plot3(visionRobot(1,:), visionRobot(2,:), visionRobot(3,:), 'r');
407 | grid on;
408 | 
409 | 
410 | 
411 | %Angles Roll
412 | figure;
413 | subplot(3,1,1);
414 | title('roll')
415 | plot(stateVector(7,:), 'b');
416 | %figure;
417 | subplot(3,1,2);
418 | plot(visionRobot(4,:), 'r');
419 | %figure;
420 | subplot(3,1,3);
421 | plot(viconEuler(1,522:end), 'g');
422 | 
423 | %Angles Pitch
424 | figure;
425 | subplot(3,1,1);
426 | title('pitch')
427 | plot(stateVector(8,:), 'b');
428 | %figure;
429 | subplot(3,1,2);
430 | plot(visionRobot(5,:), 'r');
431 | %figure;
432 | subplot(3,1,3);
433 | plot(viconEuler(2,522:end), 'g');
434 | 
435 | 
436 | %Angles Yaw
437 | figure;
438 | subplot(3,1,1);
439 | title('yaw')
440 | plot(stateVector(9,:), 'b');
441 | %figure;
442 | subplot(3,1,2);
443 | plot(visionRobot(6,:), 'r');
444 | %figure;
445 | subplot(3,1,3);
446 | plot(viconEuler(3,522:end), 'g');
447 | 
448 | 
449 | 
450 | 
451 | 
452 | % Error
453 | derp = bsxfun(@plus, viconPos, [1.5; 1.2081; 0]); %Coorindate Frame Offset
454 | theError =  derp(:, 523:1284) - stateVector(1:3,:); %i = 1284 (iteration stopped it, before divergence due to blind) (Break Loop at i = 1284)
455 | fprintf('Standard Mean Error:\n');
456 | disp(std(mean(theError)));
457 | % Error is .0396m before the huge blind divergence, the UKF does
458 | % significantly better than the 12 dim EKF over 9 dim EKF. Old .0597m
459 | 
460 | 
461 | 


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/nPointPose.m:
--------------------------------------------------------------------------------
  1 | function [ robotXYZ, orient, robot2WorldOrientation] = nPointPose( data )
  2 | %NPOINTPOSE Compute Pose in world frame based on 2D-3D correspondences.
  3 | %   Fredy Monterroza
  4 | %   data.id Contains the observed tags.
  5 | %   data contains the struct with image, pi corners, etc. data.img = april
  6 | %   tag
  7 | %   robotXYZ is world frame position (april tag mat frame)
  8 | %   orient is euler angles (orientation in the mat/world frame)
  9 | %   robot2WorldOrientation is the rotation matrix of the robot orientation
 10 | %   according to the ZXY euler angle transformation.
 11 | %   L.Quan, Linear N-Point Camera Pose Determination, IEEE Trans. on Pattern Analysis and Machine Intelligence, vol. 21, no.7, July 1999
 12 | 
 13 | global tagIDs;
 14 | global tagWorldCoords;
 15 | global cam2RobotHomoStable;
 16 | global kMat;
 17 | 
 18 | camPose = [];
 19 | quadPose = [];
 20 | 
 21 | robotXYZ = [];
 22 | %robot2WorldOrientation = [];
 23 | orient = [];
 24 | 
 25 | dataPoints = [];
 26 | M = []; %2nx9 Matrix to be build and used in Brute Force R, T calculation
 27 | if ~isempty(data.id) %only perform pose estimation when at least 1 april tag is in frame
 28 | 
 29 |     numTags = numel(data.id);
 30 |     %Extract all the available April Tags to be used as data points in
 31 |     %the 2n x 9 A matrix in x ~ A X equation for calibrated correspondences
 32 |     %Do all the bottom left points, then br, tr, tl (order dont matter)
 33 |     for tag = 1:numTags
 34 |         [~, tagLinInd] = ismember(data.id(tag), tagIDs);
 35 |         %Bottom Left
 36 |         %for bl = 1:size(data{i}.p1, 2)
 37 |         for bl = 1:1
 38 |             %xy = kMatInv * [data{i}.p1(:,bl); 1]; %Calibrated Coords
 39 |             xy = kMat \ [data.p1(:,tag); 1]; %Calibrated Coords
 40 |             X = tagWorldCoords{tagLinInd}.blx; %World Coords
 41 |             Y = tagWorldCoords{tagLinInd}.bly;
 42 |             nextPoint = [xy(1); xy(2); X; Y];
 43 |             dataPoints = [dataPoints, nextPoint];
 44 |         end
 45 |         %Bottom Right
 46 |         %for br = 1:size(data{i}.p2, 2)
 47 |         for br = 1:1
 48 |             %xy = kMatInv * [data{i}.p2(:,br); 1]; %Calibrated Coords
 49 |             xy = kMat \ [data.p2(:,tag); 1]; %Calibrated Coords
 50 |             X = tagWorldCoords{tagLinInd}.brx; %World Coords
 51 |             Y = tagWorldCoords{tagLinInd}.bry;
 52 |             nextPoint = [xy(1); xy(2); X; Y];
 53 |             dataPoints = [dataPoints, nextPoint];
 54 |         end
 55 |         %Top Right
 56 |         %for tr = 1:size(data{i}.p3, 2)
 57 |         for tr = 1:1    
 58 |             %xy = kMatInv * [data{i}.p3(:,tr); 1]; %Calibrated Coords
 59 |             xy = kMat \ [data.p3(:,tag); 1]; %Calibrated Coords
 60 |             X = tagWorldCoords{tagLinInd}.trx; %World Coords
 61 |             Y = tagWorldCoords{tagLinInd}.try;
 62 |             nextPoint = [xy(1); xy(2); X; Y];
 63 |             dataPoints = [dataPoints, nextPoint];
 64 |         end
 65 |         %Top Left
 66 |         for tl = 1:size(data.p4, 2)
 67 |         %for tl = 1:1
 68 |             %xy = kMatInv * [data{i}.p4(:,tl); 1]; %Calibrated Coords
 69 |             xy = kMat \ [data.p4(:,tag); 1]; %Calibrated Coords
 70 |             X = tagWorldCoords{tagLinInd}.tlx; %World Coords
 71 |             Y = tagWorldCoords{tagLinInd}.tly;
 72 |             nextPoint = [xy(1); xy(2); X; Y];
 73 |             dataPoints = [dataPoints, nextPoint];
 74 |         end
 75 | 
 76 |     end
 77 |     %dataPoints holds the x, y, X, Y correspondences
 78 |     %M = [];
 79 |     for point = 1:size(dataPoints, 2)
 80 |         x = dataPoints(1, point);
 81 |         y = dataPoints(2, point);
 82 |         X = dataPoints(3, point);
 83 |         Y = dataPoints(4, point);
 84 |         xY = x*Y;
 85 |         yX = y*X;
 86 |         xX = x*X;
 87 |         yY = y*Y;
 88 |         %M = [M; -X, 0, 0, -Y, 0, xY -1, 0, x; 0, -X, yX, 0, -Y, 0, 0 -1, y];
 89 |         M = [M; -X, 0, xX, -Y, 0, xY -1, 0, x; 0, -X, yX, 0, -Y, yY, 0 -1, y];
 90 |     end
 91 | 
 92 | 
 93 |     %nullM = null(M);
 94 |     [~, ~, V] = svd(M);
 95 |     nullM = V(:,end);
 96 |     %Reflection
 97 |     if  nullM(end) < 0;
 98 |         nullM = nullM * -1;
 99 |     end
100 | 
101 | 
102 |     %nullM = null(M);
103 | 
104 |     if ~isempty(nullM)
105 |         %nullM = nullM(:,end); %Take the smallest eigenvalue  rightmost vector
106 |         nullM = reshape(nullM, 3, 3);
107 |         normR1 = norm(nullM(:,1));
108 |         A = (1/normR1) * nullM; %[r1, r2, T] / ||r1|| to handle lambda scale factor
109 |         %A = normc(nullM);
110 |         r1 = A(:,1);
111 |         r2 = A(:,2);
112 |         r3 = cross(r1, r2);
113 | 
114 |         R = [r1, r2, r3]; %Orientation and
115 |         T = A(:,3); %Translation (Pose) of Camera in World Coordinates
116 | 
117 |         if ~isreal(R)
118 |             disp('NOOOOOOOOOOOOOOOOOOOOOOOOOOOO');
119 |         end
120 | 
121 | 
122 |         homoR = [R; zeros(1, 3)];
123 |         homoT = [T; 1];
124 |        
125 |         world2Robot = cam2RobotHomoStable * [homoR, homoT];
126 | 
127 |         robot2World = inv(world2Robot); %pseudo inverse? A\b?
128 |         robotXYZ = robot2World * [0; 0; 0; 1];
129 |         
130 | 
131 |         robot2WorldOrientation = robot2World(1:3, 1:3);
132 |         
133 |         orient = RotMatToRPY(robot2WorldOrientation);
134 |         roll = orient(1);
135 |         pitch = orient(2);
136 |         yaw = orient(3);
137 |         newPose = [-roll; -pitch; -yaw + pi/2; robot2World(1:3,4)];
138 | 
139 |         quadPose = [quadPose, newPose];
140 | 
141 |         robot2WorldOrientation = RPYToRotMat([-roll; -pitch; -yaw]); 
142 | 
143 |     else
144 |         disp('Empty Null Space nullM');
145 | 
146 |     end
147 | 
148 | end
149 | 
150 | 
151 | end
152 | 
153 | 


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/outputSensorLog.m:
--------------------------------------------------------------------------------
  1 | %Script to parse the data in quadData.mat and write into a .txt file.
  2 | %Will be used in C++ EKF/UKF implementation.
  3 | %Possible integration into ROS with openGL/other to plot results.
  4 | %Fredy Monterroza
  5 | 
  6 | %% Load quadData.mat: Should be in KalmanPort directory.
  7 | %quadData contains: quadLog (vicon data), quadTime (time at which vicon
  8 | %data was recorded), and sensorLog (IMU/Camera data).
  9 | addpath('MatlabFiles');
 10 | addpath('Data');
 11 | load('/Data/quadData.mat');
 12 | 
 13 | %% Parse qdLog (Vicon Data)
 14 | viconSamples = numel(qdLog);
 15 | viconEuler = zeros(3,viconSamples); %Roll Pitch Yaw
 16 | viconPos = zeros(3, viconSamples);
 17 | viconTime = zeros(1,viconSamples);
 18 | bodyLinVel = zeros(3,viconSamples);
 19 | bodyAngVel = zeros(3,viconSamples);
 20 | 
 21 | 
 22 | for viconSamp = 1:viconSamples
 23 |     viconEuler(:,viconSamp) = qdLog{viconSamp}{1}.euler;
 24 |     viconPos(:,viconSamp) = qdLog{viconSamp}{1}.pos;
 25 |     viconTime(viconSamp) = qdTimeLog{viconSamp};
 26 |     bodyLinVel(:,viconSamp) = qdLog{viconSamp}{1}.vel_body;
 27 |     bodyAngVel(:,viconSamp) = qdLog{viconSamp}{1}.omega;
 28 | end
 29 | 
 30 | %% Parse Quadrotor Sensor (IMU) Data: Note, since this is wirelessly, transmitted packets may be lost.
 31 | sensorSamples = numel(sensorLog);
 32 | sensorIMU = zeros(6, sensorSamples);
 33 | sensorTS = zeros(1,sensorSamples);
 34 | %All this data is with respect to body, different sampling than images with
 35 | %tags but the same as imageIndices.
 36 | for sensorSamp = 1:sensorSamples
 37 |     accel = sensorLog{sensorSamp}.accImu;
 38 |     angVel = sensorLog{sensorSamp}.omegaImu;
 39 |     if ~isempty(accel) %assume angvel, t also not empty if accel isn't...
 40 |         sensorIMU(:,sensorSamp) = [accel; angVel];
 41 |         sensorTS(sensorSamp) = sensorLog{sensorSamp}.t; 
 42 |     end
 43 | 
 44 | end
 45 | 
 46 | %Valid Timestamps will be >0, they are initialized/declared as 0 above.
 47 | actualIMUinds = find(sensorTS>0);
 48 | actualIMUDataReceived = sensorIMU(:,actualIMUinds); %1293 Samples
 49 | 
 50 | %% Write the data of qdLog into text files.
 51 | 
 52 | fid = fopen('viconEuler.txt', 'w');
 53 | fprintf(fid, '%.15f %.15f %.15f\n', viconEuler);
 54 | fclose(fid);
 55 | 
 56 | fid = fopen('viconPos.txt', 'w');
 57 | fprintf(fid, '%.15f %.15f %.15f\n', viconPos);
 58 | fclose(fid);
 59 | 
 60 | fid = fopen('viconTime.txt', 'w');
 61 | fprintf(fid, '%.15f\n', viconTime);
 62 | fclose(fid);
 63 | 
 64 | fid = fopen('bodyLinVel.txt', 'w');
 65 | fprintf(fid, '%.15f %.15f %.15f\n', bodyLinVel);
 66 | fclose(fid);
 67 | 
 68 | fid = fopen('bodyAngVel.txt', 'w');
 69 | fprintf(fid, '%.15f %.15f %.15f\n', bodyAngVel);
 70 | fclose(fid);
 71 | 
 72 | fid = fopen('quadTimeLog.txt', 'w'); 
 73 | fprintf(fid, '%.15f\n', quadTimeLog);
 74 | fclose(fid);
 75 | 
 76 | 
 77 | %% Write the data of sensorLog into text files.
 78 | 
 79 | sensorIMUaccel = sensorIMU(1:3,:);
 80 | sensorIMUangVel = sensorIMU(4:6,:);
 81 | 
 82 | %Format Saved in .txt is : xaccel, yaccel, zaccel, [newline]...
 83 | fid = fopen('sensorIMUaccel.txt', 'w');
 84 | fprintf(fid, '%.15f %.15f %.15f\n', sensorIMUaccel);
 85 | fclose(fid);
 86 | 
 87 | fid = fopen('sensorIMUangVel.txt', 'w');
 88 | fprintf(fid, '%.15f %.15f %.15f\n', sensorIMUangVel);
 89 | fclose(fid);
 90 | 
 91 | %Write Process Input, U, (Accelerometer) to text file
 92 | processInputAcc = processInput(1:3,:);
 93 | fid = fopen('processInputAcc.txt', 'w');
 94 | fprintf(fid, '%.15f %.15f %.15f\n', processInputAcc);
 95 | fclose(fid);
 96 | 
 97 | %Write Process Input, U, (Gyroscope) to text file
 98 | processInputOmega = processInput(4:6,:);
 99 | fid = fopen('processInputOmega.txt', 'w');
100 | fprintf(fid, '%.15f %.15f %.15f\n', processInputOmega);
101 | fclose(fid);
102 | 
103 | 
104 | %% Write nPointPose algorithm measurement to text file.
105 | 
106 | %Position and Orientation
107 | visionRobotPosData = visionRobot(1:3,:);
108 | fid = fopen('visionRobotPos.txt', 'w');
109 | fprintf(fid, '%.15f %.15f %.15f\n', visionRobotPosData);
110 | fclose(fid);
111 | 
112 | %Note: poseNPP will contain [0...0] for vector where no data is available to make pose estimate (measurement update) 
113 | %Position For Use with CPP Implementation
114 | poseNPPPosData = poseNPP(1:3,:);
115 | fid = fopen('poseNPPPos.txt', 'w');
116 | fprintf(fid, '%.15f %.15f %.15f\n', poseNPPPosData);
117 | fclose(fid);
118 | 
119 | %Position For Use with CPP Implementation
120 | poseNPPOrientData = poseNPP(4:6,:);
121 | fid = fopen('poseNPPOrient.txt', 'w');
122 | fprintf(fid, '%.15f %.15f %.15f\n', poseNPPOrientData);
123 | fclose(fid);
124 | 
125 | 
126 | %% Write Kalman Filter Data (State Vector for Trajectory and Norm of Covariance to text files.
127 | 
128 | %These are to be put into a file, they are computed as the KF runs because
129 | %that is when the robotToWorld transformation is computed.
130 | %bodyLin/AngVel to World Frame
131 | 
132 | fid = fopen('worldLinVel.txt', 'w'); 
133 | fprintf(fid, '%.15f %.15f %.15f\n', worldLinVel);
134 | fclose(fid);
135 | 
136 | fid = fopen('worldAngVel.txt', 'w');
137 | fprintf(fid, '%.15f %.15f %.15f\n', worldAngVel);
138 | fclose(fid);
139 | 
140 | %Note: Run EKF/UKF for respective state vectors, normP's
141 | %State Vector from 14 Dim EKF 
142 | stateVectorPos = stateVector(1:3,:);
143 | fid = fopen('stateVectorPos.txt', 'w'); 
144 | fprintf(fid, '%.15f %.15f %.15f\n', stateVectorPos);
145 | fclose(fid);
146 | 
147 | stateVectorVel = stateVector(4:6,:);
148 | fid = fopen('stateVectorVel.txt', 'w'); 
149 | fprintf(fid, '%.15f %.15f %.15f\n', stateVectorVel);
150 | fclose(fid);
151 | 
152 | stateVectorOrient = stateVector(7:9,:);
153 | fid = fopen('stateVectorOrient.txt', 'w'); 
154 | fprintf(fid, '%.15f %.15f %.15f\n', stateVectorOrient);
155 | fclose(fid);
156 | 
157 | fid = fopen('normP.txt', 'w'); 
158 | fprintf(fid, '%.15f\n', normP);
159 | fclose(fid);
160 | 
161 | 


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/plotUKFcppData.m:
--------------------------------------------------------------------------------
 1 | %% Read Data (Position) output from CPP UKF Implementation and Plot vs Vicon
 2 | % Data Read from a .txt file resulting from outputDataUKF.cpp
 3 | % Fredy Monterroza
 4 | 
 5 | ukfDataFile = fopen('../../CPPFiles/cppUKFData.txt');
 6 | cppPos = [];
 7 | while(~feof(ukfDataFile))
 8 |     ukfPos = fscanf(ukfDataFile, '%f %f %f', 3); %Column Order Filled
 9 |     cppPos = [cppPos, ukfPos]; %Columns are 3D-Position
10 | end
11 | fclose(ukfDataFile);
12 | 
13 | %cppUKF vs Vicon
14 | figure;
15 | plot3(cppPos(1,:), cppPos(2,:), cppPos(3,:), 'b');
16 | hold on
17 | plot3(viconPos(1,522:2015)+ 1.5, viconPos(2,522:2015)+1.2081, viconPos(3,522:2015), 'g');
18 | grid on;


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/quadUKF.m:
--------------------------------------------------------------------------------
  1 | 
  2 | function [ x, P ] = quadUKF( x, P, deltaT, z, gEqn, errVn, Vn )
  3 | % Fredy Monterroza
  4 | % Ouputs Estimated State Vector [pos vel orientation(euler angles) bax bay baz bphi btheta]
  5 | % 14 State UKF
  6 | % Code Adapted from UKF Orientation Tracking Kraft Paper. Some of the
  7 | % assumptions maintained. Here, noise is additive and added to state covariance in Sigma Point computation
  8 | % so that they are already 'disturbed'. 
  9 | % Q is Process Covariance, R is measuerment Covariance
 10 | % References:
 11 | % Julier, SJ. and Uhlmann, J.K., Unscented Filtering and Nonlinear Estimation Proceedings of the IEEE, Vol. 92, No. 3, pp.401-422, 2004. 
 12 | % Kraft, Edgar, a Quaternion Based Unscented Kalman Filter for Orientation Tracking, University of Bonn. 
 13 | 
 14 | %Q = diag([8.8749e-05,  1.6563e-04, 1.4461e-04,  1.0e-3, 1.0e-3, 1.0e-3, 6.5681e-06, 6.5455e-06, 1.3567e-05, 1.0e-3, 1.0e-3, 1.0e-3, 1e-3, 1e-3]);
 15 | Q = 5e-3*eye(14);%2e-2*eye(14);%1e-2*eye(14);
 16 | %R = diag([3.0466e-006, 5.4396e-006, 4.2754e-005, 0.0110, 0.0281, 6.3328e-04]); 
 17 | R = diag([2e-1, 2e-1, 2e-1, 1e-4, 1e-4, 1e-4]);%1.5e-1*eye(6);
 18 | 
 19 | 
 20 | %Acquire W_i/Sigma Points from Cholesky of pervious state estimate error covariance.
 21 | [sqrootPQ, p] = chol(P + Q);
 22 | 
 23 | if p > 0 %Attempt at fixing loss of PSD (probably due to floating point? (Cov Divergence Issue present in project 2.
 24 |     
 25 |     disp('cov diverge');
 26 |     [~,D] = eig(P+Q);
 27 |     D(D <= 1e-10) = 1e-6;
 28 |     D(~isreal(D)) = 1e-6;
 29 |     sqrootPQ= chol(eye(14));
 30 |     W_i = [sqrt(14)*sqrootPQ, -sqrt(14)*sqrootPQ];
 31 |     
 32 | else    
 33 |     W_i = [sqrt(14)*sqrootPQ, -sqrt(14)*sqrootPQ];
 34 | end
 35 | 
 36 | %Propogate Sigma Points through non-linear process.
 37 | X = bsxfun(@plus, W_i, x); %Sigma Points are a sampling around the mean.
 38 | X_i = zeros(14, 28); %14 x 28 dimensional vectors of sigma points
 39 | 
 40 | 
 41 | %disp(X);
 42 | global herp;
 43 | for k = 1:28
 44 |     X_i(:,k) = gEqn(Vn(1), Vn(2), Vn(3), Vn(4), Vn(5), Vn(6), X(1,k), X(10,k), X(11,k), X(12,k), X(13,k), X(14,k), X(2,k), X(3,k), X(4,k), X(5,k), X(6,k), X(7,k), X(8,k), X(9,k), ...
 45 |     deltaT, errVn(1), errVn(2), errVn(3), errVn(4), errVn(5), errVn(6), errVn(7), errVn(8), errVn(9));
 46 |     %herp = [herp, X_i(:,k)];
 47 | end
 48 | 
 49 | %Subtract Mean from Propogated Sigma Points and Obtain A-Priori Estimate Xhatbar_k = mean{Yi}
 50 | Y_i = X_i;
 51 | xhatbar_k = mean(Y_i, 2);
 52 | %global herp;
 53 | %herp = [herp, xhatbar_k];
 54 | 
 55 | %Compute {W_i_prime} set by subtracting mean from step 4 from {Y_i}. To be used for calculating a-priori state estimate covariance.
 56 | W_i_prime = bsxfun(@minus, Y_i, xhatbar_k);
 57 | 
 58 | %Step 6: Compute apriori state estimate covariance (Pbar_k) from W_i_prime.  
 59 | %Average the 28 matrices:
 60 | Pbar_k = calculateCovEquation(W_i_prime, W_i_prime);
 61 |     
 62 | %End Process Update (Dynamics Update, Prediction): If there is no
 63 | %measurement, return the a-priori estiate.
 64 | if isempty(z)
 65 |     x = xhatbar_k;
 66 |     P = Pbar_k;
 67 |     return;
 68 | end
 69 | 
 70 | 
 71 | %Predicted Observation is just Z = h(x) + delta_noise, or simply, the
 72 | %estimated measured components of the state vector, that is:
 73 | Z_i = zeros(6, 28);
 74 | for d = 1:28 
 75 | 
 76 |     Z_i(:,d) = [Y_i(1:3,d); Y_i(7:9,d)]; %Vision (no Optical Flow), can only provide, Pose. No Linear/Angular Velocity, yet.
 77 |     
 78 | end
 79 | 
 80 | %Compute Zbar_k (predicted measurement) and Vk (innovation, surprise vector)
 81 | Zbar_k = mean(Z_i, 2);
 82 | 
 83 | Zk = z; %imuData;
 84 | Vk = Zk - Zbar_k; %6x1 vectors, [acceleration; angular velocity]
 85 | 
 86 | 
 87 | %Compute Innovation Covariance (Pvv = Pzz + R) eq.69
 88 | Z_imZbar_k = bsxfun(@minus, Z_i, Zbar_k);
 89 | Pzz = calculateCovEquation(Z_imZbar_k, Z_imZbar_k);
 90 | 
 91 | Pvv = Pzz + R; %Covariance of measurement: process covariance from computing Z_i up 'till now + Measurement Covariance.
 92 | 
 93 | 
 94 | %Compute Cross Correlation Pxz from W_i_prime and Z_i sets
 95 | Pxz = calculateCovEquation(W_i_prime, Z_imZbar_k);
 96 | %Pxz = (1/12) .* (W_i_prime*Z_imZbar_k');
 97 | 
 98 | 
 99 | %Step 11: Compute Kalman Gain K_k = Pxz * Pvv^-1
100 | %disp(Pxz);
101 | %disp(inv(Pvv));
102 | K_k = Pxz * inv(Pvv); %ignore matlab's advice 'cause this is matrix multiplicaton not solving a Ax=b
103 | herp = [herp, Vk];
104 | %disp(inv(Pvv));
105 | %disp(K_k);
106 | %disp(K_k*Vk);
107 | Xhat_k = xhatbar_k + K_k*Vk;
108 | Pk = Pbar_k - K_k*Pvv*K_k'; %Estimate Error Covariance, 6x6
109 | %End Measurement Update (Correction Step)
110 | 
111 | x = Xhat_k;
112 | P = Pk;
113 | 
114 | 
115 | end
116 | 
117 | 
118 | 
119 | 
120 | 


--------------------------------------------------------------------------------
/MatlabFiles/MatlabSource/symbolicFuncs.m:
--------------------------------------------------------------------------------
  1 | %% Compute Sybolic Representation of Quadrotor Dynamics (14 Dimension EKF)
  2 | %  Fredy Monterroza
  3 | 
  4 | X = sym('X', [14 1]); %State Vector: [pos vel orientation timeVaryingAccelBias constantBiasPhiTheta]
  5 | V = sym('V', [6 1]); %Accelerometer Input
  6 | %errX = sym('errX', [6 1]); %delta, that gets added to z = h(x) + delta
  7 | errV = sym('errV', [9 1]); %IMU Error + time varying bias
  8 | deltaT = sym('deltaT');
  9 | 
 10 | temp1 = RPYToRotMat(X(7:9)); %7-9 = roll, pitch, yaw
 11 | temp2 = (V + errV(1:6)) * deltaT;
 12 | 
 13 | 
 14 | %Compute the Process Equation and Corresponding Jacobians
 15 | %In 14 Dim Version, subtract the tracked bias from accelerometer. (0-mean gaussian noise)
 16 | g = [X(1:3) + X(4:6)*deltaT + (((temp1 * (V(1:3)-X(10:12)+errV(1:3))) - [.4109;.4024;9.6343])*deltaT^2/2); ...
 17 |     X(4:6) + (((temp1 * (V(1:3)-X(10:12)+errV(1:3))) - [.4109;.4024;9.6343])*deltaT); ...
 18 |      RotMatToRPY(temp1 * RPYToRotMat(temp2(4:6))); ...
 19 |      X(10:12) + (errV(7:9)*deltaT); ...
 20 |      X(13:14)]; 
 21 | g = simplify(g); %14x1
 22 | gEqn = matlabFunction(g);
 23 |  
 24 | 
 25 | G = jacobian(g, X); %14x14
 26 | L = jacobian(g, errV); %14x9
 27 | 
 28 | gFunc = matlabFunction(G);
 29 | lFunc = matlabFunction(L);
 30 | 
 31 | 
 32 | %{
 33 | %Note: For First Sample, i = 523
 34 | 
 35 | deltaT =
 36 | 
 37 |     0.0210
 38 | U =
 39 | 
 40 |    -0.1334
 41 |    -0.0098
 42 |     9.3744
 43 |     0.0540
 44 |    -0.1100
 45 |    -0.0140
 46 | 
 47 | Xn =
 48 | 
 49 |   Columns 1 through 10
 50 | 
 51 |    -0.1976    0.0798    0.8739   -0.3618    0.3644    0.1071    0.0037   -0.0003    1.5397         0
 52 | 
 53 |   Columns 11 through 14
 54 | 
 55 |          0         0         0         0
 56 | 
 57 | Vn =
 58 | 
 59 |    -0.1334   -0.0098    9.3744    0.0540   -0.1100   -0.0140
 60 | 
 61 | errVn =
 62 | 
 63 |          0         0         0         0         0         0   -0.0185    0.0016    0.0031
 64 | 
 65 | G =
 66 | 
 67 |   Columns 1 through 10
 68 | 
 69 |     1.0000         0         0    0.0210         0         0   -0.0000   -0.0021    0.0000   -0.0000
 70 |          0    1.0000         0         0    0.0210         0    0.0021         0    0.0000    0.0002
 71 |          0         0    1.0000         0         0    0.0210   -0.0000   -0.0000   -0.0000   -0.0000
 72 |          0         0         0    1.0000         0         0   -0.0000   -0.1970    0.0028   -0.0007
 73 |          0         0         0         0    1.0000         0    0.1970         0    0.0003    0.0210
 74 |          0         0         0         0         0    1.0000   -0.0035   -0.0002   -0.0000   -0.0001
 75 |          0         0         0         0         0         0    1.0000         0   -0.0012         0
 76 |          0         0         0         0         0         0   -0.0000    1.0000    0.0023         0
 77 |          0         0         0         0         0         0    0.0012         0    1.0000         0
 78 |          0         0         0         0         0         0         0         0         0    1.0000
 79 |          0         0         0         0         0         0         0         0         0         0
 80 |          0         0         0         0         0         0         0         0         0         0
 81 |          0         0         0         0         0         0         0         0         0         0
 82 |          0         0         0         0         0         0         0         0         0         0
 83 | 
 84 |   Columns 11 through 14
 85 | 
 86 |    -0.0002   -0.0000         0         0
 87 |    -0.0000   -0.0000         0         0
 88 |     0.0000   -0.0002         0         0
 89 |    -0.0210   -0.0000         0         0
 90 |    -0.0007   -0.0001         0         0
 91 |     0.0000   -0.0210         0         0
 92 |          0         0         0         0
 93 |          0         0         0         0
 94 |          0         0         0         0
 95 |          0         0         0         0
 96 |     1.0000         0         0         0
 97 |          0    1.0000         0         0
 98 |          0         0    1.0000         0
 99 |          0         0         0    1.0000
100 | 
101 | 
102 | 
103 | L =
104 | 
105 |     0.0000    0.0002    0.0000         0         0         0         0         0         0
106 |    -0.0002    0.0000    0.0000         0         0         0         0         0         0
107 |     0.0000   -0.0000    0.0002         0         0         0         0         0         0
108 |     0.0007    0.0210    0.0000         0         0         0         0         0         0
109 |    -0.0210    0.0007    0.0001         0         0         0         0         0         0
110 |     0.0001   -0.0000    0.0210         0         0         0         0         0         0
111 |          0         0         0    0.0007    0.0210         0         0         0         0
112 |          0         0         0   -0.0210    0.0007         0         0         0         0
113 |          0         0         0    0.0000    0.0000    0.0210         0         0         0
114 |          0         0         0         0         0         0    0.0210         0         0
115 |          0         0         0         0         0         0         0    0.0210         0
116 |          0         0         0         0         0         0         0         0    0.0210
117 |          0         0         0         0         0         0         0         0         0
118 |          0         0         0         0         0         0         0         0         0
119 | 
120 | 
121 | Z =
122 | 
123 |    -0.1808
124 |     0.0950
125 |     0.8755
126 |     0.0080
127 |    -0.0035
128 |     1.5398
129 | 
130 | 
131 | K =
132 | 
133 |    1.0e-03 *
134 | 
135 |     0.0024    0.0000    0.0000         0         0         0
136 |     0.0000    0.0024    0.0000         0         0         0
137 |     0.0000    0.0000    0.0005         0         0         0
138 |     0.2321    0.0000    0.0000         0         0         0
139 |     0.0000    0.2321    0.0001         0         0         0
140 |     0.0001    0.0007    0.0522         0         0         0
141 |          0         0         0    0.4416   -0.0000    0.0005
142 |          0         0         0   -0.0000    0.0152   -0.0006
143 |          0         0         0    0.0005   -0.0000    0.4416
144 |          0         0         0         0         0         0
145 |          0         0         0         0         0         0
146 |          0         0         0         0         0         0
147 |          0         0         0         0         0         0
148 |          0         0         0         0         0         0
149 | 
150 | 
151 | Xest =
152 | 
153 |    -0.2053
154 |     0.0874
155 |     0.8761
156 |    -0.3707
157 |     0.3595
158 |     0.1016
159 |     0.0014
160 |    -0.0015
161 |     1.5394
162 |    -0.0004
163 |     0.0000
164 |     0.0001
165 |          0
166 |          0
167 | 
168 | 
169 | 
170 | P =
171 | 
172 |    1.0e-04 *
173 | 
174 |   Columns 1 through 10
175 | 
176 |     0.0000    0.0000    0.0000    0.0023    0.0000    0.0000         0         0         0         0
177 |     0.0000    0.0000    0.0000    0.0000    0.0023    0.0000         0         0         0         0
178 |     0.0000    0.0000    0.0000    0.0000    0.0000    0.0042         0         0         0         0
179 |     0.0023    0.0000    0.0000    0.2209    0.0000    0.0001         0         0         0         0
180 |     0.0000    0.0023    0.0000    0.0000    0.2209    0.0007         0         0         0         0
181 |     0.0000    0.0000    0.0042    0.0001    0.0007    0.3976         0         0         0         0
182 |          0         0         0         0         0         0    0.0044   -0.0000    0.0000         0
183 |          0         0         0         0         0         0   -0.0000    0.0044   -0.0000         0
184 |          0         0         0         0         0         0    0.0000   -0.0000    0.0044         0
185 |          0         0         0         0         0         0         0         0         0    0.2209
186 |          0         0         0         0         0         0         0         0         0         0
187 |          0         0         0         0         0         0         0         0         0         0
188 |          0         0         0         0         0         0         0         0         0         0
189 |          0         0         0         0         0         0         0         0         0         0
190 | 
191 |   Columns 11 through 14
192 | 
193 |          0         0         0         0
194 |          0         0         0         0
195 |          0         0         0         0
196 |          0         0         0         0
197 |          0         0         0         0
198 |          0         0         0         0
199 |          0         0         0         0
200 |          0         0         0         0
201 |          0         0         0         0
202 |          0         0         0         0
203 |     0.2209         0         0         0
204 |          0    0.3976         0         0
205 |          0         0         0         0
206 |          0         0         0         0
207 | 
208 | normP =
209 | 
210 |    1.0e-04 *
211 | 
212 |          0    0.3976
213 | 
214 | %}
215 | 
216 | 
217 | 
218 | 
219 | 
220 | 
221 | 
222 | 
223 | 
224 | 
225 | 
226 | 
227 | 
228 | 
229 | 
230 | 
231 | 
232 | 
233 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | ##Kalman Filter Port
 2 | ###Porting of Matlab Based Code for the State Estimation of a Quadrotor into C++/ROS framework. (UKF/EKF).
 3 | 
 4 | ####State Vector for both Kalman Filter implementations is 14 Dimensional: 
 5 | ####[position, velocity, orientation, imu accelerometer bias, roll/pitch bias]
 6 | 
 7 | ####Implementation uses Boost 1.49, C++11/STL, and ROS Hyrdo
 8 | 
 9 | ######Information
10 | The Kalman Filter is an optimal estimator. If
11 | the noise of the system and observations can be modeled as
12 | Gaussian, then the Kalman Filter minimizes the mean square
13 | error of the estimate. In addition, the filter is recursive and can
14 | thus provide state estimates as new data becomes available. If
15 | you have a good estimate, then combining the filter with a
16 | pre-process step of gain learning can achieve a reliable system. 
17 | The purpose of the project was to fly a quadrotor using
18 | either the Extended Kalman Filter or the Unscented Kalman
19 | Filter with an IMU and single camera serving as input to
20 | the system. Once a good state estimator was developed, this
21 | would be used in conjunction with a PD controller which
22 | would use the position and velocity estimates to calculate the
23 | required thrusts and moments to reach the desired position.
24 | The nanoplus quadrotor has an onboard attitude controller
25 | that runs at a higher frequency than the position and velocity
26 | controller. This means the orientation estimate would only
27 | be kept as an exercise of the kalman filter since the attitude
28 | controller requires a larger rate of input than is available
29 | (The IMU and Camera data were synchronized at 33Hz).
30 | Having obtained a good controller and good state estimate,
31 | the quadrotor could track a desired trajectory. If you imagine
32 | a situation where the quadrotor would be required to follow a
33 | desired trajectory as accurately as possible (say, surveillance
34 | duty) then it would be necessary to learn an optimal set of
35 | gains. Here, only the Kalman Filters are provided.
36 | 
37 | ######Files
38 | [.cpp Source] (/CPPFiles)
39 | [Matlab Source] (/MatlabFiles/MatlabSource)
40 | 
41 | [CPP Unscented Kalman Filter] (/CPPFiles/fmUKF.cpp)
42 | [Matlab Unscented Kalman Filter] (/MatlabFiles/MatlabSource/quadUKF.m)
43 | 
44 | [CPP UKF Loop] (/CPPFiles/quadStateEst.cpp)
45 | [Matlab UKF Loop] (/MatlabFiles/MatlabSource/imuUKF.m)
46 | 
47 | [Matlab EKF] (/MatlabFiles/MatlabSource/final650.m)
48 | 
49 | [Matlab N-Point Pose Algorithm] (/MatlabFiles/MatlabSource/nPointPose.m)
50 | 
51 | [Symbolic Functions] (/MatlabFiles/MatlabSource/symbolicFuncs.m)
52 | [Ouput Sensor Log Script] (/MatlabFiles/MatlabSource/outputSensorLog.m)
53 | [Read into STL Vectors] (/CPPFiles/inputData.cpp)
54 | 
55 | [Boost LU Factorization Matrix Inversion] (/CPPFiles/InvertMatrix.hpp)
56 | [Boost Cholesky Decomposition] (/CPPFiles/cholesky.cpp)
57 | 
58 | [Data Collected Wirelessly From Quadrotor] (/Data)
59 | 
60 | ######Using RViz
61 | [ROS Variant] (/ukfROS)
62 | 
63 | Open a terminal and run 'roscore'. In a second terminal, open rviz with
64 | 'rosrun rviz rviz &' and change the 'Fixed Frame' to '/quad_World' 
65 | under 'Global Options'. Then add a marker display type, which should 
66 | be subscribed to the 'visualization_marker' topic. Finally, in a third
67 | terminal, execute ./quadStateEstROS and you should see the path of the
68 | quadrotor in Rviz.
69 | 
70 | The full command to compile and build the source is provided here for reference:
71 | g++ -Wall -c quadStateEstROS.cpp -o quadStateEstROS -std=c++11 
72 | -I/opt/ros/hydro/include 
73 | -L/opt/ros/hydro/lib /opt/ros/hydro/lib/libroscpp_serialization.so 
74 | -Wl,-rpath,/opt/ros/hydro/lib -lroscpp -lrosconsole -lrostime
75 | 
76 | 
77 | 


--------------------------------------------------------------------------------
/presentationKF.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gitfredy/KalmanPort/7e5af03d02de7a2113737e9d8611a67b5584a43d/presentationKF.pdf


--------------------------------------------------------------------------------
/ukfROS/InvertMatrix.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef INVERT_MATRIX_HPP
 2 | #define INVERT_MATRIX_HPP
 3 | 
 4 | /*
 5 |  * Modified from http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?LU_Matrix_Inversion
 6 |  * By, Fredrik Orderud.
 7 |  */
 8 | 
 9 | 
10 | // REMEMBER to update "lu.hpp" header includes from boost-CVS
11 | //#include <boost/numeric/ublas/vector.hpp>
12 | //#include <boost/numeric/ublas/vector_proxy.hpp>
13 | //#include <boost/numeric/ublas/matrix.hpp>
14 | //#include <boost/numeric/ublas/triangular.hpp>
15 | #include <boost/numeric/ublas/lu.hpp>
16 | //#include <boost/numeric/ublas/io.hpp>
17 | 
18 | //namespace ublas = boost::numeric::ublas;
19 | 
20 | /* Matrix inversion routine.
21 | Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
22 | template<class T>
23 | bool InvertMatrix (const boost::numeric::ublas::matrix<T>& input,boost::numeric::ublas::matrix<T>& inverse) {
24 | //using namespace boost::numeric::ublas;
25 | 
26 | typedef boost::numeric::ublas::permutation_matrix<std::size_t> pmatrix;
27 | 
28 | // create a working copy of the input
29 | boost::numeric::ublas::matrix<T> A(input);
30 | 
31 | // create a permutation matrix for the LU-factorization
32 | pmatrix pm(A.size1());
33 | 
34 | // perform LU-factorization
35 | int res = lu_factorize(A,pm);
36 |     if( res != 0 ) return false;
37 | 
38 | // create identity matrix of "inverse"
39 | inverse.assign(boost::numeric::ublas::identity_matrix<T>(A.size1()));
40 | 
41 | // backsubstitute to get the inverse
42 | lu_substitute(A, pm, inverse);
43 | return true;
44 | }
45 | #endif //INVERT_MATRIX_HPP
46 | 


--------------------------------------------------------------------------------
/ukfROS/cholesky.hpp:
--------------------------------------------------------------------------------
  1 | /** -*- c++ -*- \file cholesky.hpp \brief cholesky decomposition */
  2 | /*
  3 |  -   begin                : 2005-08-24
  4 |  -   copyright            : (C) 2005 by Gunter Winkler, Konstantin Kutzkow
  5 |  -   email                : guwi17@gmx.de
  6 | 
  7 |     This library is free software; you can redistribute it and/or
  8 |     modify it under the terms of the GNU Lesser General Public
  9 |     License as published by the Free Software Foundation; either
 10 |     version 2.1 of the License, or (at your option) any later version.
 11 | 
 12 |     This library is distributed in the hope that it will be useful,
 13 |     but WITHOUT ANY WARRANTY; without even the implied warranty of
 14 |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 15 |     Lesser General Public License for more details.
 16 | 
 17 |     You should have received a copy of the GNU Lesser General Public
 18 |     License along with this library; if not, write to the Free Software
 19 |     Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 20 | 
 21 | */
 22 | 
 23 | #ifndef _H_CHOLESKY_HPP_
 24 | #define _H_CHOLESKY_HPP_
 25 | 
 26 | 
 27 | #include <cassert>
 28 | 
 29 | #include <boost/numeric/ublas/vector.hpp>
 30 | #include <boost/numeric/ublas/vector_proxy.hpp>
 31 | 
 32 | #include <boost/numeric/ublas/matrix.hpp>
 33 | #include <boost/numeric/ublas/matrix_proxy.hpp>
 34 | 
 35 | #include <boost/numeric/ublas/vector_expression.hpp>
 36 | #include <boost/numeric/ublas/matrix_expression.hpp>
 37 | 
 38 | #include <boost/numeric/ublas/triangular.hpp>
 39 | 
 40 | namespace ublasCHOL = boost::numeric::ublas;
 41 | 
 42 | 
 43 | /** \brief decompose the symmetric positive definit matrix A into product L L^T.
 44 |  *
 45 |  * \param MATRIX type of input matrix 
 46 |  * \param TRIA type of lower triangular output matrix
 47 |  * \param A square symmetric positive definite input matrix (only the lower triangle is accessed)
 48 |  * \param L lower triangular output matrix 
 49 |  * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
 50 |  */
 51 | template < class MATRIX, class TRIA >
 52 | size_t cholesky_decompose(const MATRIX& A, TRIA& L)
 53 | {
 54 |   using namespace ublasCHOL;
 55 | 
 56 |   typedef typename MATRIX::value_type T;
 57 |   
 58 |   assert( A.size1() == A.size2() );
 59 |   assert( A.size1() == L.size1() );
 60 |   assert( A.size2() == L.size2() );
 61 | 
 62 |   const size_t n = A.size1();
 63 |   
 64 |   for (size_t k=0 ; k < n; k++) {
 65 |         
 66 |     double qL_kk = A(k,k) - inner_prod( project( row(L, k), range(0, k) ),
 67 |                                         project( row(L, k), range(0, k) ) );
 68 |     
 69 |     if (qL_kk <= 0) {
 70 |       return 1 + k;
 71 |     } else {
 72 |       double L_kk = sqrt( qL_kk );
 73 |       L(k,k) = L_kk;
 74 |       
 75 |       matrix_column<TRIA> cLk(L, k);
 76 |       project( cLk, range(k+1, n) )
 77 |         = ( project( column(A, k), range(k+1, n) )
 78 |             - prod( project(L, range(k+1, n), range(0, k)), 
 79 |                     project(row(L, k), range(0, k) ) ) ) / L_kk;
 80 |     }
 81 |   }
 82 |   return 0;      
 83 | }
 84 | 
 85 | 
 86 | /** \brief decompose the symmetric positive definit matrix A into product L L^T.
 87 |  *
 88 |  * \param MATRIX type of matrix A
 89 |  * \param A input: square symmetric positive definite matrix (only the lower triangle is accessed)
 90 |  * \param A output: the lower triangle of A is replaced by the cholesky factor
 91 |  * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
 92 |  */
 93 | template < class MATRIX >
 94 | size_t cholesky_decompose(MATRIX& A)
 95 | {
 96 |   using namespace ublasCHOL;
 97 | 
 98 |   typedef typename MATRIX::value_type T;
 99 |   
100 |   const MATRIX& A_c(A);
101 | 
102 |   const size_t n = A.size1();
103 |   
104 |   for (size_t k=0 ; k < n; k++) {
105 |         
106 |     double qL_kk = A_c(k,k) - inner_prod( project( row(A_c, k), range(0, k) ),
107 |                                           project( row(A_c, k), range(0, k) ) );
108 |     
109 |     if (qL_kk <= 0) {
110 |       return 1 + k;
111 |     } else {
112 |       double L_kk = sqrt( qL_kk );
113 |       
114 |       matrix_column<MATRIX> cLk(A, k);
115 |       project( cLk, range(k+1, n) )
116 |         = ( project( column(A_c, k), range(k+1, n) )
117 |             - prod( project(A_c, range(k+1, n), range(0, k)), 
118 |                     project(row(A_c, k), range(0, k) ) ) ) / L_kk;
119 |       A(k,k) = L_kk;
120 |     }
121 |   }
122 |   return 0;      
123 | }
124 | 
125 | #if 0
126 |   using namespace ublasCHOL;
127 | 
128 |     // Operations:
129 |     //  n * (n - 1) / 2 + n = n * (n + 1) / 2 multiplications,
130 |     //  n * (n - 1) / 2 additions
131 | 
132 |     // Dense (proxy) case
133 |     template<class E1, class E2>
134 |     void inplace_solve (const matrix_expression<E1> &e1, vector_expression<E2> &e2,
135 |                         lower_tag, column_major_tag) {
136 |       std::cout << " is_lc ";
137 |         typedef typename E2::size_type size_type;
138 |         typedef typename E2::difference_type difference_type;
139 |         typedef typename E2::value_type value_type;
140 | 
141 |         BOOST_UBLAS_CHECK (e1 ().size1 () == e1 ().size2 (), bad_size ());
142 |         BOOST_UBLAS_CHECK (e1 ().size2 () == e2 ().size (), bad_size ());
143 |         size_type size = e2 ().size ();
144 |         for (size_type n = 0; n < size; ++ n) {
145 | #ifndef BOOST_UBLAS_SINGULAR_CHECK
146 |             BOOST_UBLAS_CHECK (e1 () (n, n) != value_type/*zero*/(), singular ());
147 | #else
148 |             if (e1 () (n, n) == value_type/*zero*/())
149 |                 singular ().raise ();
150 | #endif
151 |             value_type t = e2 () (n) / e1 () (n, n);
152 |             e2 () (n) = t;
153 |             if (t != value_type/*zero*/()) {
154 |               project( e2 (), range(n+1, size) )
155 |                 .minus_assign( t * project( column( e1 (), n), range(n+1, size) ) );
156 |             }
157 |         }
158 |     }
159 | #endif
160 | 
161 | 
162 | 
163 | 
164 | 
165 | /** \brief decompose the symmetric positive definit matrix A into product L L^T.
166 |  *
167 |  * \param MATRIX type of matrix A
168 |  * \param A input: square symmetric positive definite matrix (only the lower triangle is accessed)
169 |  * \param A output: the lower triangle of A is replaced by the cholesky factor
170 |  * \return nonzero if decompositon fails (the value ist 1 + the numer of the failing row)
171 |  */
172 | template < class MATRIX >
173 | size_t incomplete_cholesky_decompose(MATRIX& A)
174 | {
175 |   using namespace ublasCHOL;
176 | 
177 |   typedef typename MATRIX::value_type T;
178 |   
179 |   // read access to a const matrix is faster
180 |   const MATRIX& A_c(A);
181 | 
182 |   const size_t n = A.size1();
183 |   
184 |   for (size_t k=0 ; k < n; k++) {
185 |     
186 |     double qL_kk = A_c(k,k) - inner_prod( project( row( A_c, k ), range(0, k) ),
187 |                                           project( row( A_c, k ), range(0, k) ) );
188 |     
189 |     if (qL_kk <= 0) {
190 |       return 1 + k;
191 |     } else {
192 |       double L_kk = sqrt( qL_kk );
193 | 
194 |       // aktualisieren
195 |       for (size_t i = k+1; i < A.size1(); ++i) {
196 |         T* Aik = A.find_element(i, k);
197 | 
198 |         if (Aik != 0) {
199 |           *Aik = ( *Aik - inner_prod( project( row( A_c, k ), range(0, k) ),
200 |                                       project( row( A_c, i ), range(0, k) ) ) ) / L_kk;
201 |         }
202 |       }
203 |         
204 |       A(k,k) = L_kk;
205 |     }
206 |   }
207 |         
208 |   return 0;
209 | }
210 | 
211 | 
212 | 
213 | 
214 | /** \brief solve system L L^T x = b inplace
215 |  *
216 |  * \param L a triangular matrix
217 |  * \param x input: right hand side b; output: solution x
218 |  */
219 | template < class TRIA, class VEC >
220 | void
221 | cholesky_solve(const TRIA& L, VEC& x, ublasCHOL::lower)
222 | {
223 |   using namespace ublasCHOL;
224 | //   ::inplace_solve(L, x, lower_tag(), typename TRIA::orientation_category () );
225 |   inplace_solve(L, x, lower_tag() );
226 |   inplace_solve(trans(L), x, upper_tag());
227 | }
228 | 
229 | 
230 | #endif
231 | 


--------------------------------------------------------------------------------
/ukfROS/fmUKF.cpp:
--------------------------------------------------------------------------------
  1 | /*
  2 |  * Unscented Kalman Filter
  3 |  * State: Position, Velocity, Orientation, Accelerometer Bias, Roll/Pitch Bias
  4 |  * Fredy Monterroza 
  5 |  * 
  6 | */
  7 | 
  8 | #include <iostream>
  9 | //#include <fstream>
 10 | #include <stdio.h>
 11 | #include <stdlib.h>
 12 | #include <string.h>
 13 | 
 14 | #include <math.h>
 15 | 
 16 | #include <boost/numeric/ublas/vector.hpp>
 17 | #include <boost/numeric/ublas/matrix.hpp>
 18 | #include <boost/numeric/ublas/io.hpp>
 19 | 
 20 | 
 21 | #include "cholesky.hpp"
 22 | #include "InvertMatrix.hpp"
 23 | 
 24 | using std::cout;
 25 | using std::cin;
 26 | using std::endl;
 27 | 
 28 | using namespace boost::numeric;
 29 | 
 30 | ublas::vector<double> gEqn(ublas::vector<double>, ublas::vector<double>, ublas::vector<double>, double);
 31 | ublas::matrix<double> RPYtoRotMat(double, double, double); //Roll, Pitch, Yaw
 32 | double atanFM(double, double);
 33 | ublas::matrix<double> calculateCovEquation(ublas::matrix<double>, ublas::matrix<double>);
 34 | 
 35 | ublas::vector<double> sigmaPoint(14);
 36 | extern ublas::matrix<double> Q;
 37 | extern ublas::matrix<double> R;
 38 | extern ublas::vector<double> Xest;
 39 | extern int kfIteration;
 40 | 
 41 | /*
 42 |  * Using the Unscented Kalman Filter, compute a state estimate from the gEqn() quadrotor dynamics
 43 |  * and update this apriori state estimate whenever a measurement is available from the nPointPose
 44 |  * algorithm. 
 45 |  * @param stateVector Container to hold the computed state estimates.
 46 |  * @param x Previous State Estimate
 47 |  * @param P Previous State Covariance
 48 |  * @param deltaT Sample Rate, time at which data collected on quadrotor
 49 |  * @param z Measurement provided by N-Point Pose Algorithm 
 50 |  * @param errVn Disturbance used in tracking accelerometer bias
 51 |  * @param Vn Control input from IMU. 
 52 |  */
 53 | void fmUKF (std::vector<dataVec>& stateVector, ublas::vector<double> x, ublas::matrix<double>& P, double deltaT, ublas::vector<double> z, ublas::vector<double> errVn, ublas::vector<double> Vn)
 54 | 
 55 | {
 56 | 	//cout << "fmUKF" << endl;
 57 | 
 58 | 	ublas::matrix<double> sumPQ = P + Q;
 59 | 	ublas::matrix<double> L(14, 14);
 60 | 	size_t decompFail = cholesky_decompose(sumPQ, L);
 61 | 	ublas::matrix<double> W_i(14,28);
 62 | 	ublas::vector<double> aprioriStateEst(14);
 63 | 	std::fill(aprioriStateEst.begin(), aprioriStateEst.end(), 0.0);
 64 | 	ublas::matrix<double> X_i(14,28);
 65 | 
 66 | 	if (decompFail == 0) {
 67 | 		//Capture the Covariance
 68 | 		subrange(W_i, 0, 14, 0, 14) = sqrt(14.0) * L;
 69 | 		subrange(W_i, 0, 14, 14, 28) = -sqrt(14.0) * L;
 70 | 
 71 | 		//Add the previous mean Xest to complete Sigma Points
 72 | 		for(unsigned i = 0; i < W_i.size2(); ++i){ //bsxfun(@plus)
 73 | 			column(W_i, i) = column(W_i, i) + x;
 74 | 		}
 75 | 		//cout << W_i << endl;
 76 | 
 77 | 
 78 | 	} else { 
 79 | 		cout << "Require Square Symmertric Positive Definite Input" << endl;
 80 | 		//Reset L to identity. 
 81 | 		for(ublas::matrix<double>::iterator1 iter1L = L.begin1(); iter1L != L.end1(); ++iter1L){ //Row by row
 82 | 			for(ublas::matrix<double>::iterator2 iter2L = iter1L.begin(); iter2L != iter1L.end(); ++iter2L){ //Go from col to the next col for fixed row
 83 | 				if (iter1L.index1() == iter2L.index2()){
 84 | 					*iter2L = 0.1225; //Cholesky of Initial P + Q //1;
 85 | 				} else {
 86 | 					*iter2L = 0;
 87 | 				}
 88 | 			}
 89 | 	    }
 90 | 		subrange(W_i, 0, 14, 0, 14) = sqrt(14.0) * L;
 91 | 		subrange(W_i, 0, 14, 14, 28) = -sqrt(14.0) * L;
 92 | 
 93 | 		//Add the previous mean Xest to complete Sigma Points
 94 | 		for(unsigned i = 0; i < W_i.size2(); ++i){ //bsxfun(@plus)
 95 | 			column(W_i, i) = column(W_i, i) + x;
 96 | 		}
 97 | 	}
 98 | 	//cout << W_i << endl;
 99 | 
100 | 	//Propogate the Sigma Points
101 | 	ublas::vector<double> propogatedSigma(14);
102 | 	for(unsigned i = 0; i < W_i.size2(); ++i){
103 | 		sigmaPoint = column(W_i, i);
104 | 		propogatedSigma = gEqn(sigmaPoint, errVn, Vn, deltaT);
105 | 		//cout << kfIteration << endl;
106 | 		//cout << propogatedSigma << endl;
107 | 	    column(X_i, i) = propogatedSigma;
108 | 	    aprioriStateEst += propogatedSigma;
109 | 	}
110 | 	
111 | 	aprioriStateEst /= X_i.size2(); //Mean
112 | 
113 | 	//W_i is W_i_prime (Propogated Sigma Points - Mean subtracted)
114 | 	for(unsigned i = 0; i < X_i.size2(); ++i){
115 | 		column(W_i, i) = column(X_i, i) - aprioriStateEst;
116 | 	}
117 | 
118 | 	ublas::matrix<double> Pbar_k = calculateCovEquation(W_i, W_i);
119 | 
120 | 	//No Measurement Update Available
121 | 	if (z(0)+z(1)+z(2)+z(3)+z(4)+z(5) == 0){
122 | 		P = Pbar_k; 
123 | 		dataVec tempVec;
124 | 		for(unsigned i = 0; i < aprioriStateEst.size(); ++i){
125 | 			tempVec.push_back(aprioriStateEst(i));
126 | 			Xest(i) = aprioriStateEst(i);
127 | 		}
128 | 		stateVector.push_back(tempVec);
129 | 		//cout << "No Measurement Update Available to Correct Apriori Estimate" << endl;
130 | 		return;
131 | 	}
132 | 
133 | 	//Predicted Observation Zbar_k, Observation subset Z_i
134 | 	ublas::matrix<double> Z_i(6, 28); //Pos, Orient
135 | 	ublas::vector<double> Zbar_k(6);
136 | 	Zbar_k(0) = 0; Zbar_k(1) = 0; Zbar_k(2) = 0; Zbar_k(3) = 0; Zbar_k(4) = 0; Zbar_k(5) = 0;
137 | 	for(unsigned i = 0; i < X_i.size2(); ++i){
138 | 		Z_i(0, i) = X_i(0, i); Z_i(1, i) = X_i(1, i); Z_i(2, i) = X_i(2, i);
139 | 		Z_i(3, i) = X_i(6, i); Z_i(4, i) = X_i(7, i); Z_i(5, i) = X_i(8, i);
140 | 		Zbar_k += column(Z_i, i);
141 | 	}
142 | 	//Note: These will vary from matlab since this involves propogated sigma points, random walk model.
143 | 	Zbar_k /= 28;
144 | 
145 | 	//Innovation Vector (Residual between Actual and Expected Measurement
146 | 	ublas::vector<double> Vk = z - Zbar_k;
147 | 
148 | 	//Recycle, Reuse, Reduce: Observation subset with mean subtracted.
149 | 	for(unsigned i = 0; i < Z_i.size2(); ++i){
150 | 		column(Z_i, i) = column(Z_i, i) - Zbar_k;
151 | 	}
152 | 
153 | 
154 | 	//Innovation Covariance
155 | 	ublas::matrix<double> Pvv = calculateCovEquation(Z_i, Z_i);
156 | 	Pvv += R;
157 | 
158 | 	//Cross Correlation
159 | 	ublas::matrix<double> Pxz = calculateCovEquation(W_i, Z_i);
160 | 
161 | 	ublas::matrix<double> PvvInv(Pvv.size1(), Pvv.size2());
162 | 	if(InvertMatrix(Pvv, PvvInv)){ //via LU Factorization
163 | 		
164 | 		//Kalman Gain
165 | 		ublas::matrix<double> K_k = prod(Pxz, PvvInv);
166 | 		
167 | 		//Measurement Updated State Estimate
168 | 		ublas::vector<double> tempProd = prod(K_k, Vk); //These placeholders are necessary to store result of prod()
169 | 		ublas::vector<double> aposterioriStateEst = aprioriStateEst + tempProd;
170 | 		
171 | 		dataVec tempVec;
172 | 		for(unsigned i = 0; i < aposterioriStateEst.size(); ++i){
173 | 			tempVec.push_back(aposterioriStateEst(i));
174 | 			Xest(i) = aposterioriStateEst(i);
175 | 		}
176 | 		stateVector.push_back(tempVec);
177 | 
178 | 		ublas::matrix<double> holdProd = prod(Pvv, trans(K_k)); 
179 | 		ublas::matrix<double> tempProd2 = prod(K_k, holdProd);
180 | 		P = Pbar_k - tempProd2;//prod(K_k, holdProd);
181 | 
182 | 	}else {cout << "Pvv Approaching Singularity" << endl;}
183 | 
184 | 
185 | 
186 | }
187 | 
188 | /*
189 |  * Quadrotor (Non-Linear) Dynamics
190 |  * @param sigmaPoint Sigma Point captured by Cholesky Decomposition and Previous State Estimate
191 |  * @param errVn Disturbance used to track accelerometer bias/
192 |  * @param Vn Control Input from IMU
193 |  * @param deltaT Difference in time between sample collection on quadrotor.
194 |  * @return propogated sigma point.
195 |  */
196 | ublas::vector<double> gEqn(ublas::vector<double> sigmaPoint, ublas::vector<double> errVn, ublas::vector<double> Vn, double deltaT){	
197 | 	
198 | 	ublas::vector<double> propState(14);
199 | 
200 | 	ublas::matrix<double> rotMat = RPYtoRotMat(sigmaPoint(6), sigmaPoint(7), sigmaPoint(8));
201 | 
202 | 	//Acceleration
203 | 	ublas::vector<double> tempAccInput(3); tempAccInput(0) = Vn(0) - sigmaPoint(9); tempAccInput(1) = Vn(1) - sigmaPoint(10); tempAccInput(2) = Vn(2) - sigmaPoint(11); //IMU Accel - Tracked Bias
204 | 	ublas::vector<double> worldAcc = prod(rotMat, tempAccInput);
205 | 	worldAcc(0) = worldAcc(0) - .4109; worldAcc(1) = worldAcc(1) - .4024; worldAcc(2) = worldAcc(2) - 9.6343; //Subtract Gravity 
206 | 
207 | 	//Position
208 | 	propState(0) = sigmaPoint(0) + (sigmaPoint(3)*deltaT) + (0.5*worldAcc(0)*deltaT*deltaT);
209 | 	propState(1) = sigmaPoint(1) + (sigmaPoint(4)*deltaT) + (0.5*worldAcc(1)*deltaT*deltaT);
210 | 	propState(2) = sigmaPoint(2) + (sigmaPoint(5)*deltaT) + (0.5*worldAcc(2)*deltaT*deltaT);
211 | 
212 | 	//Velocity
213 | 	propState(3) = sigmaPoint(3) + (worldAcc(0)*deltaT);
214 | 	propState(4) = sigmaPoint(4) + (worldAcc(1)*deltaT);
215 | 	propState(5) = sigmaPoint(5) + (worldAcc(2)*deltaT);
216 | 
217 | 	//Orientation: Radians
218 | 	ublas::matrix<double> gyroIntegration = RPYtoRotMat(Vn(3)*deltaT, Vn(4)*deltaT, Vn(5)*deltaT);
219 | 	ublas::matrix<double> tempQuadOrientMat = prod(rotMat, gyroIntegration);
220 | 
221 | 	propState(6) = asin(tempQuadOrientMat(1,2));
222 | 	propState(7) = atanFM(-tempQuadOrientMat(0,2)/cos(propState(6)),tempQuadOrientMat(2,2)/cos(propState(6)));
223 | 	propState(8) = atanFM(-tempQuadOrientMat(1,0)/cos(propState(6)),tempQuadOrientMat(1,1)/cos(propState(6)));
224 | 
225 | 
226 | 	//Accelerometer Bias (Random Walk)
227 | 	propState(9) = sigmaPoint(9) + (errVn(6)*deltaT); propState(10) = sigmaPoint(10) + (errVn(7)*deltaT); propState(11) = sigmaPoint(11) + (errVn(8)*deltaT);
228 | 
229 | 	propState(12) = sigmaPoint(12); propState(13) = sigmaPoint(13);
230 | 
231 | 	return propState;
232 | 
233 | }
234 | 
235 | /*
236 |  * Return Rotation Matrix along ZXY
237 |  * @param phi Roll angle in radians
238 |  * @param theta Pitch angle in radians
239 |  * @param psi Yaw angle in radians
240 |  * @param Orientation of Quadrotor in world frame
241 |  */
242 | ublas::matrix<double> RPYtoRotMat(double phi, double theta, double psi){
243 | 	ublas::matrix<double> rotMat(3,3);
244 | 	rotMat(0,0) = cos(psi)*cos(theta) - sin(phi)*sin(psi)*sin(theta);
245 | 	rotMat(0,1) = cos(theta)*sin(psi) + cos(psi)*sin(phi)*sin(theta);
246 | 	rotMat(0,2) = -cos(phi)*sin(theta);
247 | 
248 | 	rotMat(1,0) =  -cos(phi)*sin(psi);
249 | 	rotMat(1,1) = cos(phi)*cos(psi); 
250 | 	rotMat(1,2) = sin(phi);
251 | 
252 | 	rotMat(2,0) = cos(psi)*sin(theta) + cos(theta)*sin(phi)*sin(psi);
253 | 	rotMat(2,1) = sin(psi)*sin(theta) - cos(psi)*cos(theta)*sin(phi);
254 | 	rotMat(2,2) = cos(phi)*cos(theta);
255 | 
256 | 	return rotMat;
257 | }
258 | 
259 | /*
260 |  * atan used with Matlab Symbolic Explressions, replicated here for comparison.
261 |  * @return Inverse Tangent in radians.
262 |  */
263 | double atanFM(double y, double x){
264 | 	return  2*atan(y / ( sqrt((x*x)+(y*y)) + x ));	
265 | }
266 | 
267 | /*
268 |  * Covariance Matrix calculated as weighted/averaged outer product of input points/vectors.
269 |  * @return Covariance Matrix obtained between A, B vectors.
270 |  */
271 | ublas::matrix<double> calculateCovEquation(ublas::matrix<double> A, ublas::matrix<double> B){
272 | 
273 | 	//ublas::matrix<double> covMat(14, 14);
274 | 	ublas::matrix<double> covMat(A.size1(), B.size1());
275 | 	for(unsigned i = 0; i < covMat.size1(); ++i){
276 | 		for(unsigned j = 0; j < covMat.size2(); ++j){ 
277 | 			covMat(i, j) = 0.0;
278 | 		}
279 | 	}
280 | 
281 | 	//ublas::vector<double> transposeVec;
282 | 	for(unsigned i = 0; i < A.size2(); ++i){
283 | 		covMat += outer_prod(column(A, i), column(B, i));//outer_prod(colA, colB);//outer_prod(column(A, i), column(B, i));
284 | 	}
285 | 	covMat /= 28;
286 | 
287 | 	return covMat;
288 | }
289 | 
290 | 
291 | 
292 | 
293 | 
294 |    
295 |       
296 | 


--------------------------------------------------------------------------------
/ukfROS/inputData.cpp:
--------------------------------------------------------------------------------
  1 | /* 
  2 |    This file is to read in the quadData.mat cell structure containing quadLog, quadTime, and sensorLog and parse 
  3 |    it into corresponding data structures necessary for implementation of the EKF/UKF.
  4 |    Fredy Monterroza
  5 | */
  6 | 
  7 | #include <iostream>
  8 | #include <fstream>
  9 | #include <stdio.h>
 10 | #include <stdlib.h>
 11 | #include <string.h>
 12 | #include <iomanip>
 13 | #include <sstream>
 14 | 
 15 | #include <algorithm>
 16 | #include <vector>
 17 | #include <map>
 18 | #include <list>
 19 | 
 20 | 
 21 | 
 22 | //using namespace std;
 23 | using std::cout;
 24 | using std::cin;
 25 | using std::endl;
 26 | using std::vector;
 27 | using std::string;
 28 | using std::stringstream;
 29 | using std::list;
 30 | using std::ifstream;
 31 | 
 32 | 
 33 | typedef vector<double> dataVec;
 34 | 
 35 | dataVec parseStringForData(string&);
 36 | 
 37 | std::vector<dataVec> sensorLogAccImu;
 38 | std::vector<dataVec> sensorLogOmegaImu; 
 39 | std::vector<double> quadTimeLog;
 40 | std::vector<dataVec> visionRobotPos; 
 41 | std::vector<dataVec> visionRobotOrient; 
 42 | std::vector<dataVec> viconEuler; 
 43 | std::vector<dataVec> viconPos;
 44 | std::list<double> viconTime;
 45 | 
 46 | //void inputData(void);
 47 | //int main() { //void inputData()
 48 | void inputData(){
 49 | 
 50 |  //-----Read Process Input (Control Input, IMU Accelerometer)-----
 51 |  //vector<dataVec> sensorLogAccImu; 
 52 |  string inSensorAccel;
 53 |  ifstream inFileSensorAccel ("../Data/processInputAcc.txt");
 54 |  if(inFileSensorAccel.is_open()){
 55 |   while(getline(inFileSensorAccel, inSensorAccel)){
 56 |    dataVec imuAccel;
 57 |    imuAccel = parseStringForData(inSensorAccel);
 58 |    sensorLogAccImu.push_back(imuAccel);
 59 |   }
 60 |   inFileSensorAccel.close();
 61 |  }else{
 62 |   cout << "Error Opening File" << endl;
 63 |  }
 64 | 
 65 |  /* 
 66 |  for(vector<dataVec>::iterator imuAccelIter = sensorLogAccImu.begin(); imuAccelIter != sensorLogAccImu.end(); ++imuAccelIter){
 67 |   for(dataVec::iterator accelImuIter = imuAccelIter->begin(); accelImuIter != imuAccelIter->end(); ++accelImuIter){
 68 |    cout << *accelImuIter << " ";
 69 |   }
 70 |   cout << endl;
 71 |  }
 72 |  */
 73 |  
 74 |  //-----Read Process Input (Control Input, IMU Gyroscope, Angular Velocity)-----
 75 |  //vector<dataVec> sensorLogOmegaImu; 
 76 |  string inSensorOmega;
 77 |  ifstream inFileSensorOmega ("../Data/processInputOmega.txt");
 78 |  if(inFileSensorOmega.is_open()){
 79 |   while(getline(inFileSensorOmega, inSensorOmega)){
 80 |    dataVec imuOmega;
 81 |    imuOmega = parseStringForData(inSensorOmega);
 82 |    sensorLogOmegaImu.push_back(imuOmega);
 83 |   }
 84 |   inFileSensorOmega.close();
 85 |  }else{
 86 |   cout << "Error Opening File" << endl;
 87 |  }
 88 |  /*
 89 |  for(vector<dataVec>::iterator imuOmegaIter = sensorLogOmegaImu.begin(); imuOmegaIter != sensorLogOmegaImu.end(); ++imuOmegaIter){
 90 |   for(dataVec::iterator omegaImuIter = imuOmegaIter->begin(); omegaImuIter != imuOmegaIter->end(); ++omegaImuIter){
 91 |    cout << *omegaImuIter << " ";
 92 |   }
 93 |   cout << endl;
 94 |  }
 95 |  */
 96 |  
 97 |   //-----Read Quad Sensor Times------
 98 |  string inQuadTime;
 99 |  double inQuadTimeNum;
100 |  ifstream inFileQuadTime ("../Data/quadTimeLog.txt");
101 |  if(inFileQuadTime.is_open()){
102 |   while(getline(inFileQuadTime, inQuadTime)){
103 |    inQuadTimeNum = stod(inQuadTime);
104 |    quadTimeLog.push_back(inQuadTimeNum);
105 |   }
106 |   inFileQuadTime.close();
107 |  }else {
108 |  cout << "Error Opening File" << endl;
109 |  }
110 |  
111 |  
112 |  //-----Read poseNPPPos (Measurement Update, Position Output From nPointPose Algorithm)-----
113 |  //vector<dataVec> visionRobotPos; 
114 |  string inVisionRobotPos;
115 |  ifstream inFileVisionRobotPos ("../Data/poseNPPPos.txt");
116 |  if(inFileVisionRobotPos.is_open()){
117 |   while(getline(inFileVisionRobotPos, inVisionRobotPos)){
118 |    dataVec robotPos;
119 |    robotPos = parseStringForData(inVisionRobotPos);
120 |    visionRobotPos.push_back(robotPos);
121 |   }
122 |   inFileVisionRobotPos.close();
123 |  }else{
124 |   cout << "Error Opening File" << endl;
125 |  }
126 |  /*
127 |  for(vector<dataVec>::iterator visionPosIter = visionRobotPos.begin(); visionPosIter != visionRobotPos.end(); ++visionPosIter){
128 |   for(dataVec::iterator posVisionIter = visionPosIter->begin(); posVisionIter != visionPosIter->end(); ++posVisionIter){
129 |    cout << *posVisionIter << " ";
130 |   }
131 |   cout << endl;
132 |  }
133 | */
134 | 
135 |  //-----Read poseNPPOrient (Measurement Update, Orientation Output From nPointPose Algorithm)-----
136 |  //vector<dataVec> visionRobotOrient; 
137 |  string inVisionRobotOrient;
138 |  ifstream inFileVisionRobotOrient ("../Data/poseNPPOrient.txt");
139 |  if(inFileVisionRobotOrient.is_open()){
140 |   while(getline(inFileVisionRobotOrient, inVisionRobotOrient)){
141 |    dataVec robotOrient;
142 |    robotOrient = parseStringForData(inVisionRobotOrient);
143 |    visionRobotOrient.push_back(robotOrient);
144 |   }
145 |   inFileVisionRobotOrient.close();
146 |  }else{
147 |   cout << "Error Opening File" << endl;
148 |  }
149 |  /*
150 |  for(vector<dataVec>::iterator visionOrientIter = visionRobotOrient.begin(); visionOrientIter != visionRobotOrient.end(); ++visionOrientIter){
151 |   for(dataVec::iterator orientVisionIter = visionOrientIter->begin(); orientVisionIter != visionOrientIter->end(); ++orientVisionIter){
152 |    cout << *orientVisionIter << " ";
153 |   }
154 |   cout << endl;
155 |  }
156 | */
157 | 
158 | 
159 | 
160 |  //-----Read Vicon Euler Angles-----
161 |  //vector<dataVec> viconEuler; 
162 |  string inEuler;
163 |  //ifstream inFileEuler ("quadData/viconEuler.txt");
164 |  ifstream inFileEuler ("../Data/viconEuler.txt");
165 |  if(inFileEuler.is_open()){
166 |   while(getline(inFileEuler, inEuler)){
167 |    dataVec euler;
168 |    euler = parseStringForData(inEuler);
169 |    viconEuler.push_back(euler);
170 |   }
171 |   inFileEuler.close();
172 |  }else{
173 |   cout << "Error Opening File" << endl;
174 |  }
175 |  /*
176 |  for(vector<dataVec>::iterator eulerIter = viconEuler.begin(); eulerIter != viconEuler.end(); ++eulerIter){
177 |   for(dataVec::iterator iterAngs = eulerIter->begin(); iterAngs != eulerIter->end(); ++iterAngs){
178 |    cout << *iterAngs << " ";
179 |   }
180 |   cout << endl;
181 |  }
182 |  */
183 | 
184 |  //-----Read Vicon Position-----
185 |  //vector<dataVec> viconPos; 
186 |  string inPos;
187 |  //ifstream inFilePos ("quadData/viconPos.txt");
188 |  ifstream inFilePos ("../Data/viconPos.txt");
189 |  if(inFilePos.is_open()){
190 |   while(getline(inFilePos, inPos)){
191 |    dataVec pos;
192 |    pos = parseStringForData(inPos);
193 |    viconPos.push_back(pos);
194 |   }
195 |   inFilePos.close();
196 |  }else{
197 |   cout << "Error Opening File" << endl;
198 |  }
199 |  
200 |  /*
201 |  for(vector<dataVec>::iterator posIter = viconPos.begin(); posIter != viconPos.end(); ++posIter){
202 |   for(dataVec::iterator iterPos = posIter->begin(); iterPos != posIter->end(); ++iterPos){
203 |    cout << *iterPos << " ";
204 |   }
205 |   cout << endl;
206 |  }
207 |  */
208 | 
209 | 
210 |  //-----Read Vicon Times------
211 |  //list<double> viconTime;
212 |  string inTime;
213 |  double inTimeNum;
214 |  //ifstream inFileTime ("quadData/viconTime.txt");
215 |  ifstream inFileTime ("../Data/viconTime.txt");
216 |  if(inFileTime.is_open()){
217 |   while(getline(inFileTime, inTime)){
218 |    //inTimeNum = atof(inTime.c_str());
219 |    //printf("%f\n", inTimeNum);
220 |    inTimeNum = stod(inTime);
221 |    //cout << setprecision(17) << inTimeNum << endl;
222 |    viconTime.push_back(inTimeNum);
223 |  
224 |   }
225 |   inFileTime.close();
226 |  }else {
227 |  cout << "Error Opening File" << endl;
228 |  }
229 |  /*
230 |  for (list<double>::iterator vtIter = viconTime.begin(); vtIter != viconTime.end(); ++vtIter){
231 |   cout << setprecision(17) << *vtIter << endl;
232 |  }
233 |  */
234 |  
235 | }
236 | 
237 | dataVec parseStringForData(string& inData){
238 |  string dataElement;
239 |  stringstream dataStream(inData);
240 |  dataVec dataVector;
241 |  double dataElementNum;
242 |  while(dataStream >> dataElement){
243 |   dataElementNum = stod(dataElement);
244 |   //cout << setprecision(17) << dataElementNum << endl;
245 |   dataVector.push_back(dataElementNum);
246 |  }
247 | 
248 |  return dataVector;
249 | 
250 | }
251 | 
252 | /*
253 | 
254 |  //-----Read Sensor (IMU) Accelerometer-----
255 |  vector<dataVec> sensorLogAccImu; 
256 |  string inSensorAccel;
257 |  ifstream inFileSensorAccel ("../Data/sensorIMUaccel.txt");
258 |  if(inFileSensorAccel.is_open()){
259 |   while(getline(inFileSensorAccel, inSensorAccel)){
260 |    dataVec imuAccel;
261 |    imuAccel = parseStringForData(inSensorAccel);
262 |    sensorLogAccImu.push_back(imuAccel);
263 |   }
264 |   inFileSensorAccel.close();
265 |  }else{
266 |   cout << "Error Opening File" << endl;
267 |  }
268 |  
269 |  for(vector<dataVec>::iterator imuAccelIter = sensorLogAccImu.begin(); imuAccelIter != sensorLogAccImu.end(); ++imuAccelIter){
270 |   for(dataVec::iterator accelImuIter = imuAccelIter->begin(); accelImuIter != imuAccelIter->end(); ++accelImuIter){
271 |    cout << *accelImuIter << " ";
272 |   }
273 |   cout << endl;
274 |  }
275 |  
276 | 
277 |  //-----Read Sensor (IMU) Gyroscope (Angular Velocity)-----
278 |  vector<dataVec> sensorLogOmegaImu; 
279 |  string inSensorOmega;
280 |  ifstream inFileSensorOmega ("../Data/sensorIMUangVel.txt");
281 |  if(inFileSensorOmega.is_open()){
282 |   while(getline(inFileSensorOmega, inSensorOmega)){
283 |    dataVec imuOmega;
284 |    imuOmega = parseStringForData(inSensorOmega);
285 |    sensorLogOmegaImu.push_back(imuOmega);
286 |   }
287 |   inFileSensorOmega.close();
288 |  }else{
289 |   cout << "Error Opening File" << endl;
290 |  }
291 |  
292 |  for(vector<dataVec>::iterator imuOmegaIter = sensorLogOmegaImu.begin(); imuOmegaIter != sensorLogOmegaImu.end(); ++imuOmegaIter){
293 |   for(dataVec::iterator omegaImuIter = imuOmegaIter->begin(); omegaImuIter != imuOmegaIter->end(); ++omegaImuIter){
294 |    cout << *omegaImuIter << " ";
295 |   }
296 |   cout << endl;
297 |  }
298 | 
299 |  
300 |   //-----Read visionRobotPos (Position Output From nPointPose Algorithm)-----
301 |  vector<dataVec> visionRobotPos; 
302 |  string inVisionRobotPos;
303 |  ifstream inFileVisionRobotPos ("../Data/visionRobotPos.txt");
304 |  if(inFileVisionRobotPos.is_open()){
305 |   while(getline(inFileVisionRobotPos, inVisionRobotPos)){
306 |    dataVec robotPos;
307 |    robotPos = parseStringForData(inVisionRobotPos);
308 |    visionRobotPos.push_back(robotPos);
309 |   }
310 |   inFileVisionRobotPos.close();
311 |  }else{
312 |   cout << "Error Opening File" << endl;
313 |  }
314 |  
315 |  for(vector<dataVec>::iterator visionPosIter = visionRobotPos.begin(); visionPosIter != visionRobotPos.end(); ++visionPosIter){
316 |   for(dataVec::iterator posVisionIter = visionPosIter->begin(); posVisionIter != visionPosIter->end(); ++posVisionIter){
317 |    cout << *posVisionIter << " ";
318 |   }
319 |   cout << endl;
320 |  }
321 |  
322 | */
323 | 
324 | 


--------------------------------------------------------------------------------
/ukfROS/quadStateEstROS:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/gitfredy/KalmanPort/7e5af03d02de7a2113737e9d8611a67b5584a43d/ukfROS/quadStateEstROS


--------------------------------------------------------------------------------
/ukfROS/quadStateEstROS.cpp:
--------------------------------------------------------------------------------
  1 | #include <iostream>
  2 | #include <stdio.h>
  3 | #include <stdlib.h>
  4 | #include <string.h>
  5 | 
  6 | #include <math.h>
  7 | 
  8 | #include <boost/numeric/ublas/vector.hpp>
  9 | #include <boost/numeric/ublas/matrix.hpp>
 10 | #include <boost/numeric/ublas/io.hpp>
 11 | #include <boost/math/complex/acos.hpp>
 12 | #include <boost/math/complex/asin.hpp>
 13 | #include <boost/math/complex/atan.hpp>
 14 | #include <boost/random.hpp>
 15 | #include <boost/random/normal_distribution.hpp>
 16 | 
 17 | #include <ros/ros.h>
 18 | #include <visualization_msgs/Marker.h>
 19 | 
 20 | #include "inputData.cpp"
 21 | #include "fmUKF.cpp"
 22 | //#include "outputDataUKF.cpp"
 23 | 
 24 | using std::cout;
 25 | using std::cin;
 26 | using std::endl;
 27 | 
 28 | using namespace boost::numeric;
 29 | 
 30 | //Data Containers from Sensor (IMU), nPointPose Measurement, Vicon
 31 | extern std::vector<dataVec> sensorLogAccImu;
 32 | extern std::vector<dataVec> sensorLogOmegaImu; 
 33 | extern std::vector<double> quadTimeLog;
 34 | extern std::vector<dataVec> visionRobotPos; 
 35 | extern std::vector<dataVec> visionRobotOrient; 
 36 | extern std::vector<dataVec> viconEuler; 
 37 | extern std::vector<dataVec> viconPos;
 38 | extern std::list<double> viconTime;
 39 | 
 40 | //Store State Estimates 
 41 | std::vector<dataVec> stateVector;
 42 | 
 43 | //Computation Variables
 44 | ublas::vector<double> Xest(14);
 45 | ublas::matrix<double> P(14,14);
 46 | double deltaT;
 47 | double prevT;
 48 | ublas::vector<double> Z(6);
 49 | ublas::vector<double> errVn(9);
 50 | ublas::vector<double> Vn(6);
 51 | 
 52 | 
 53 | //Process Covariance (Q) and Measurement Covariance (R)
 54 | ublas::matrix<double> Q(14,14);
 55 | ublas::matrix<double> R(6,6);
 56 | 
 57 | int kfIteration = 1;
 58 | 
 59 | int main( int argc, char** argv )
 60 | {
 61 | 
 62 |     //For use with random walk model of accelerometer bias.
 63 | 	boost::mt19937 rng;
 64 | 	boost::normal_distribution<> xBias(0.0, 0.0094);
 65 |     boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > xBiasSamp(rng, xBias);
 66 |     boost::normal_distribution<> yBias(0.0,  0.0129);
 67 |     boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > yBiasSamp(rng, yBias);
 68 |     boost::normal_distribution<> zBias(0.0, 0.0120);
 69 |     boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > zBiasSamp(rng, zBias);
 70 | 	
 71 | 	
 72 | 	//Initialize ROS Node
 73 | 	
 74 | 	ros::init(argc, argv, "quadStateEstROS");
 75 | 	ros::NodeHandle n;
 76 | 	ros::Rate r(10);
 77 | 	ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);
 78 | 	visualization_msgs::Marker points; //State Estimates (Position) pushed into this marker
 79 |     //Frame Used by RViz
 80 | 	points.header.frame_id = "/quad_World";
 81 |     points.header.stamp = ros::Time::now();
 82 |     points.ns = "quadStateEstROS";
 83 |     points.action = visualization_msgs::Marker::ADD;
 84 |     points.pose.orientation.w = 1.0;
 85 |     
 86 |     points.id = 0;
 87 |     points.type = visualization_msgs::Marker::POINTS;
 88 |     
 89 |     points.scale.x = 0.05;
 90 |     points.scale.y = 0.05;
 91 |    
 92 |     points.color.g = 1.0f;
 93 |     points.color.a = 1.0;
 94 |     
 95 |     geometry_msgs::Point p; //3D World Visualization of Quadrotor Pos
 96 | 	
 97 | 	
 98 | 	inputData();
 99 | 	
100 | 	//Fill Process Covariance Q
101 | 	for(ublas::matrix<double>::iterator1 iter1Q = Q.begin1(); iter1Q != Q.end1(); ++iter1Q){ //Row by row
102 | 		for(ublas::matrix<double>::iterator2 iter2Q = iter1Q.begin(); iter2Q != iter1Q.end(); ++iter2Q){ //Go from col to the next col for fixed row
103 | 			if (iter1Q.index1() == iter2Q.index2()){
104 | 				*iter2Q = .005;
105 | 			} else {
106 | 				*iter2Q = 0;
107 | 			}
108 | 		}
109 | 	 } 
110 | 	 
111 | 	 //Fill Measurement Covariance R
112 | 	 int traceIndex = 1;
113 | 	 for(ublas::matrix<double>::iterator1 iter1R = R.begin1(); iter1R != R.end1(); ++iter1R){ //Row by row
114 | 		for(ublas::matrix<double>::iterator2 iter2R = iter1R.begin(); iter2R != iter1R.end(); ++iter2R){ //Go from col to the next col for fixed row
115 | 			if (iter1R.index1() == iter2R.index2()){
116 | 				if (traceIndex <=3) { *iter2R = .2; }
117 | 				else { *iter2R = .0001; }
118 | 				traceIndex++;
119 | 			}
120 | 		}
121 | 	 } 
122 | 	 
123 | 	 
124 | 	 
125 | 	/*
126 | 	 * Initialize UKF
127 | 	 */ 
128 | 	
129 | 	//First State estimate formed from known onboard sensor values and pose estimate/transforations of N-Point Pose. (Refer to the Matlab source)
130 | 	Xest(0) = -0.197612876747667;
131 | 	Xest(1) = 0.079773798179542;
132 | 	Xest(2) = 0.873867606945072;
133 | 	Xest(3) = -0.361840566716804;
134 | 	Xest(4) = 0.364435964213824;
135 | 	Xest(5) = 0.107057884736583;
136 | 	Xest(6) = 0.003720102419965;
137 | 	Xest(7) = -.0003130927657911031;
138 | 	Xest(8) = 1.539666077119250;
139 | 	Xest(9) = 0.0;
140 | 	Xest(10) = 0.0;
141 | 	Xest(11) = 0.0;
142 | 	Xest(12) = 0.0;
143 | 	Xest(13) = 0.0;
144 | 	
145 | 	 
146 | 	//Initialize prevU: Previous Control Input to be used when data dropped. (Maintain Heading when Blind). 
147 | 	//Note: All the used control inputs already collected in sensorLogAccImu, sensorLogOmegaImu, simply iterate.
148 | 	
149 | 	//Initialize prevTime: First Time Stamp
150 | 	vector<double>::iterator qtIter = quadTimeLog.begin();
151 | 	prevT = *qtIter++;
152 | 	
153 | 	
154 | 	//Save the Position Estimate Component for use with RViz/3D trajectory visualization
155 | 	dataVec tempVec;
156 | 	for(unsigned i = 0; i < Xest.size(); ++i){
157 | 		tempVec.push_back(Xest(i));
158 | 	}
159 | 	stateVector.push_back(tempVec);
160 | 	
161 | 	//TODO: 2-Norm of Matrix P
162 | 	
163 | 	//First State Covariance Matrix P
164 | 	for(ublas::matrix<double>::iterator1 iter1P = P.begin1(); iter1P != P.end1(); ++iter1P){ //Row by row
165 | 		for(ublas::matrix<double>::iterator2 iter2P = iter1P.begin(); iter2P != iter1P.end(); ++iter2P){ //Go from col to the next col for fixed row
166 | 			if (iter1P.index1() == iter2P.index2()){
167 | 				*iter2P = .01;
168 | 			} else {
169 | 				*iter2P = 0;
170 | 			}
171 | 		}
172 | 	 } 
173 | 	 
174 | 	
175 | 	//Use Accelerometer Input Iterator as Marker for End of Loop
176 | 	vector<dataVec>::iterator accIter = sensorLogAccImu.begin();
177 | 	vector<dataVec>::iterator omegaIter = sensorLogOmegaImu.begin();
178 | 	vector<dataVec>::iterator pNPPIter = visionRobotPos.begin();
179 | 	vector<dataVec>::iterator oNPPIter = visionRobotOrient.begin();
180 | 	dataVec::iterator accInput;
181 | 	dataVec::iterator gyroInput;
182 | 	dataVec::iterator nppPos;
183 | 	dataVec::iterator nppOrient;
184 | 	++accIter; ++omegaIter; ++pNPPIter; ++oNPPIter; //First input/NPP unused here, but used in Matlab
185 | 	
186 | 	/*
187 | 	 * Unscented Kalman Filter Loop 
188 | 	 */ 
189 | 	
190 | 	while(accIter != sensorLogAccImu.end()){
191 | 		
192 | 		accInput = accIter->begin();
193 | 		gyroInput = omegaIter->begin();
194 | 		for(unsigned i = 0; i < (Vn.size()/2); ++i){
195 | 			Vn(i) = *accInput++;
196 | 			Vn(i+3) = *gyroInput++;
197 | 		}
198 | 		deltaT = *qtIter - prevT;
199 | 		errVn(0) = 0; errVn(1) = 0;  errVn(2) = 0;  errVn(3) = 0;  errVn(4) = 0;  errVn(5) = 0;  
200 | 		errVn(6) = xBiasSamp(); errVn(7) = yBiasSamp(); errVn(8) = zBiasSamp();
201 | 		
202 | 		nppPos = pNPPIter->begin();
203 | 		nppOrient = oNPPIter->begin();
204 | 		for(unsigned i = 0; i < (Z.size()/2); ++i){
205 | 			Z(i) = *nppPos++;
206 | 			Z(i+3) = *nppOrient++;
207 | 		}
208 | 		
209 | 		cout << kfIteration << endl;
210 | 		cout << Xest << endl;
211 | 		//Push Position Estimate into Visualization Point Marker
212 | 		p.x = Xest(0);
213 | 		p.y = Xest(1);
214 | 		p.z = Xest(2);
215 | 		points.points.push_back(p);
216 | 		fmUKF(stateVector, Xest, P, deltaT, Z, errVn, Vn);
217 | 
218 | 		//Update prevTime
219 | 		prevT = *qtIter;
220 | 		
221 | 		//TODO: Save/Evaluate normP
222 | 		
223 | 		//Advance Iterators, kfIteration
224 | 		++qtIter; ++accIter; ++omegaIter; ++pNPPIter; ++oNPPIter;
225 | 		kfIteration++;
226 | 	}
227 | 	cout << "Number of Kalman Filter Iterations = " << kfIteration << endl;
228 | 	
229 | 	//Write Position Estimates to Text File (Not if Using ROS, simply use RViz)
230 | 	//outputDataUKF(stateVector);
231 | 
232 |   //Publish Data for use with RViz, terminate with Ctrl-C or add ros::shutdown()
233 |   while (ros::ok())
234 |   {
235 |  
236 |     //Publish Data For ROS RViz to Visualize	
237 |     points.lifetime = ros::Duration();
238 |     marker_pub.publish(points);
239 |     
240 |     r.sleep();
241 | 
242 |   }
243 | 
244 | 	return 0;
245 | }
246 |   
247 |   
248 |   
249 | 


--------------------------------------------------------------------------------