GraftUKFAttitude.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* 
00031  * Author: Chad Rockey
00032  */
00033 
00034  #include <graft/GraftUKFAttitude.h>
00035  #include <ros/console.h>
00036 
00037  GraftUKFAttitude::GraftUKFAttitude(){
00038         graft_state_.setZero();
00039         graft_state_(0,0) = 1.0; // Normalize quaternion
00040         graft_control_.setZero();
00041         graft_covariance_.setIdentity();
00042         Q_.setZero();
00043  }
00044 
00045 GraftUKFAttitude::~GraftUKFAttitude(){
00046 
00047 }
00048 
00049 VectorXd verticalConcatenate(VectorXd& m, VectorXd& n){
00050         if(m.rows() == 0){
00051                 return n;
00052         }
00053         VectorXd out;
00054         out.resize(m.rows()+n.rows(), m.cols());
00055         out << m,n;
00056         return out;
00057 }
00058 
00059 MatrixXd verticalConcatenate(MatrixXd& m, MatrixXd& n){
00060         if(m.rows() == 0){
00061                 return n;
00062         }
00063         MatrixXd out;
00064         out.resize(m.rows()+n.rows(), m.cols());
00065         out << m,n;
00066         return out;
00067 }
00068 
00069 MatrixXd matrixSqrt(MatrixXd matrix){ 
00070         // Use LLT Cholesky decomposiion to create stable Matrix Sqrt
00071         return Eigen::LLT<Eigen::MatrixXd>(matrix).matrixL();
00072 }
00073 
00074 std::vector<MatrixXd > generateSigmaPoints(MatrixXd state, MatrixXd covariance, double lambda){
00075         std::vector<MatrixXd > out;
00076 
00077         double gamma = std::sqrt((state.rows()+lambda));
00078         MatrixXd sig_sqrt = gamma*matrixSqrt(covariance);
00079 
00080         // i = 0, push back state as is
00081         out.push_back(state);
00082 
00083         // i = 1,...,n
00084         for(size_t i = 1; i <= state.rows(); i++){
00085                 out.push_back(state + sig_sqrt.col(i-1));
00086         }
00087 
00088         // i = n + 1,...,2n
00089         for(size_t i = state.rows() + 1; i <= 2*state.rows(); i++){
00090                 out.push_back(state - sig_sqrt.col(i-(state.rows()+1)));
00091         }
00092 
00093         return out;
00094 }
00095 
00096 MatrixXd meanFromSigmaPoints(std::vector<MatrixXd >& sigma_points, double n, double lambda){
00097         double weight_zero = lambda / (n + lambda);
00098         MatrixXd out = weight_zero * sigma_points[0];
00099         double weight_i = 1.0/(2*(n + lambda));
00100         for(size_t i = 1; i <= 2*n; i++){
00101                 out = out + weight_i * sigma_points[i];
00102         }
00103         return out;
00104 }
00105 
00107 MatrixXd covarianceFromSigmaPoints(std::vector<MatrixXd >& sigma_points, MatrixXd& mean, MatrixXd process_noise, double n, double alpha, double beta, double lambda){
00108         double cov_weight_zero = lambda / (n + lambda) + (1 - alpha*alpha + beta);
00109         MatrixXd out = cov_weight_zero * (sigma_points[0] - mean) * (sigma_points[0] - mean).transpose();
00110         double weight_i = 1.0/(2*(n + lambda));
00111         for(size_t i = 1; i <= 2*n; i++){
00112                 out = out + weight_i  * (sigma_points[i] - mean) * (sigma_points[i] - mean).transpose();
00113         }
00114         return out+process_noise;
00115 }
00116 
00117 MatrixXd crossCovariance(std::vector<MatrixXd >& sigma_points, MatrixXd& mean, std::vector<MatrixXd >& meas_sigma_points, MatrixXd& meas_mean, double alpha, double beta, double lambda){
00118         double n = sigma_points[0].rows();
00119         double cov_weight_zero = lambda / (n + lambda) + (1 - alpha*alpha + beta);
00120         MatrixXd out = cov_weight_zero * (sigma_points[0] - mean) * (meas_sigma_points[0] - meas_mean).transpose();
00121         double weight_i = 1.0/(2*(n + lambda));
00122         for(size_t i = 1; i <= 2*n; i++){
00123                 out = out + weight_i  * (sigma_points[i] - mean) * (meas_sigma_points[i] - meas_mean).transpose();
00124         }
00125         return out;
00126 }
00127 
00128 Matrix<double, 4, 4> quaternionUpdateMatrix(const double wx, const double wy, const double wz){
00129         Matrix<double, 4, 4> out;
00130         out <<   0,  wx,  wy,  wz,
00131                -wx,   0, -wz,  wy,
00132                -wy,  wz,   0, -wx,
00133                -wz, -wy,  wx,   0;
00134    return out;
00135 }
00136 
00137 Matrix<double, 4, 1> unitQuaternion(const Matrix<double, 4, 1>& q){
00138         double q_mag = std::sqrt(q(0)*q(0) + q(1)*q(1) + q(2)*q(2) + q(3)*q(3));
00139         return q / q_mag;
00140 }
00141 
00142 Matrix<double, 4, 1> updatedQuaternion(const Matrix<double, 4, 1>& q, const double wx, const double wy, const double wz, double dt){
00143         Matrix<double, 4, 1> out;
00144         Matrix<double, 4, 4> I = Matrix<double, 4, 4>::Identity();
00145         double s = 1.0/2.0 * std::sqrt(wx*wx*dt*dt + wy*wy*dt*dt + wz*wz*dt*dt);
00146         double k = 0.0;
00147         double q_mag = std::sqrt(q(0)*q(0) + q(1)*q(1) + q(2)*q(2) + q(3)*q(3));
00148         double err = 1.0 - q_mag*q_mag;
00149         
00150         double correction_factor = 1 - 1.0/2.0*s*s + 1.0/24.0*s*s*s*s + k * dt * err; // Cosine taylor series
00151         MatrixXd correction = I*(correction_factor);
00152         double update_factor = 1.0/2.0*dt*(1.0 - 1.0/6.0*s*s + 1.0/120.0*s*s*s*s); // Sinc taylor series
00153         Matrix<double, 4, 4> quaterion_update_matrix = quaternionUpdateMatrix(wx, wy, wz);
00154         MatrixXd update = update_factor*quaterion_update_matrix;
00155         out = (correction-update)*q;
00156 
00157 
00158         double out_mag = std::sqrt(out(0)*out(0) + out(1)*out(1) + out(2)*out(2) + out(3)*out(3));
00159         double out_err = 1.0 - out_mag*out_mag;
00160 
00161         return out;
00162 }
00163 
00164 MatrixXd GraftUKFAttitude::f(MatrixXd x, double dt){
00165         Matrix<double, SIZE, 1> out;
00166         out.setZero();
00167         Matrix<double, 4, 1> new_q = updatedQuaternion(x.block(0, 0, 4, 1), x(4), x(5), x(6), dt);
00168         out.block(0, 0, 4, 1) = new_q;
00169         out(4) = x(4); // wx
00170         out(5) = x(5); // wy
00171         out(6) = x(6); // wz
00172         return out;
00173 }
00174 
00175 graft::GraftState::ConstPtr stateMsgFromMatrix(const MatrixXd& state){
00176         graft::GraftState::Ptr out(new graft::GraftState());
00177         out->pose.orientation.w = state(0);
00178         out->pose.orientation.x = state(1);
00179         out->pose.orientation.y = state(2);
00180         out->pose.orientation.z = state(3);
00181         out->twist.angular.x = state(4);
00182         out->twist.angular.y = state(5);
00183         out->twist.angular.z = state(6);
00184         return out;
00185 }
00186 
00187 std::vector<MatrixXd > GraftUKFAttitude::predict_sigma_points(std::vector<MatrixXd >& sigma_points, double dt){
00188         std::vector<MatrixXd > out;
00189         for(size_t i = 0; i < sigma_points.size(); i++){
00190                 out.push_back(f(sigma_points[i], dt));
00191         }
00192         return out;
00193 }
00194 
00195 graft::GraftStatePtr GraftUKFAttitude::getMessageFromState(){
00196         return GraftUKFAttitude::getMessageFromState(graft_state_, graft_covariance_);
00197 }
00198 
00199 graft::GraftStatePtr GraftUKFAttitude::getMessageFromState(Matrix<double, SIZE, 1>& state, Matrix<double, SIZE, SIZE>& covariance){
00200         graft::GraftStatePtr msg(new graft::GraftState());
00201         msg->pose.orientation.w = state(0);
00202         msg->pose.orientation.x = state(1);
00203         msg->pose.orientation.y = state(2);
00204         msg->pose.orientation.z = state(3);
00205         msg->twist.angular.x = state(4);
00206         msg->twist.angular.y = state(5);
00207         msg->twist.angular.z = state(6);
00208 
00209         for(size_t i = 0; i < SIZE*SIZE; i++){
00210                 msg->covariance[i] = covariance(i);
00211         }
00212         return msg;
00213 }
00214 
00215 VectorXd addElementToVector(const VectorXd& vec, const double element){
00216         if(vec.size() == 0){
00217                 VectorXd out(1);
00218                 out(0) = element;
00219                 return out;
00220         }
00221         VectorXd out(vec.size() + 1);
00222         out << vec, element;
00223         return out;
00224 }
00225 
00226 MatrixXd addElementToColumnMatrix(const MatrixXd& mat, const double element){
00227         MatrixXd out(mat.rows() + 1, 1);
00228         MatrixXd small(1, 1);
00229         small(0,0) = element;
00230         if(mat.rows() == 0){
00231                 return small;
00232         }
00233         out << mat, small;
00234         return out;
00235 }
00236 
00237 geometry_msgs::Vector3 normalized_acceleration(const geometry_msgs::Vector3& accel){
00238         geometry_msgs::Vector3 out;
00239         double mag = std::sqrt(accel.x*accel.x + accel.y*accel.y + accel.z*accel.z);
00240         out.x = accel.x / mag;
00241         out.y = accel.y / mag;
00242         out.z = accel.z / mag;
00243         return out;
00244 }
00245 
00246 // Returns measurement vector
00247 VectorXd getMeasurements(const std::vector<boost::shared_ptr<GraftSensor> >& topics, const std::vector<MatrixXd>& predicted_sigma_points, std::vector<MatrixXd>& output_measurement_sigmas, MatrixXd& output_innovation_covariance){
00248         VectorXd actual_measurement;
00249         output_measurement_sigmas.clear();
00250         VectorXd innovation_covariance_diagonal;
00251         innovation_covariance_diagonal.resize(0);
00252         // Convert the predicted_sigma_points into messages
00253         std::vector<graft::GraftState::ConstPtr> predicted_sigma_msgs;
00254         for(size_t i = 0; i < predicted_sigma_points.size(); i++){
00255                 predicted_sigma_msgs.push_back(stateMsgFromMatrix(predicted_sigma_points[i]));
00256                 output_measurement_sigmas.push_back(MatrixXd());
00257         }
00258 
00259         
00260         // For each topic
00261         for(size_t i = 0; i < topics.size(); i++){
00262                 // Get the measurement msg and covariance
00263                 graft::GraftSensorResidual::ConstPtr meas = topics[i]->z();
00264                 // Get the predicted measurements
00265                 std::vector<graft::GraftSensorResidual::ConstPtr> residuals_msgs;
00266                 for(size_t j = 0; j < predicted_sigma_msgs.size(); j++){
00267                         graft::GraftSensorResidual::Ptr msg = topics[i]->h(*predicted_sigma_msgs[j]);
00268                         residuals_msgs.push_back(msg);
00269 
00270                 }
00271                 // Assemble outputs for this topic
00272                 if(meas == NULL){ // Timeout or not received or invalid, skip
00273                         continue;
00274                 }
00275                 // Angular Velocity X
00276                 if(meas->twist_covariance[21] > 1e-20){
00277                         actual_measurement = addElementToVector(actual_measurement, meas->twist.angular.x);
00278                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[21]);
00279                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00280                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.angular.x);
00281                         }
00282                 }
00283                 // Angular Velocity Y
00284                 if(meas->twist_covariance[28] > 1e-20){
00285                         actual_measurement = addElementToVector(actual_measurement, meas->twist.angular.y);
00286                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[28]);
00287                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00288                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.angular.y);
00289                         }
00290                 }
00291                 // Angular Velocity Z
00292                 if(meas->twist_covariance[35] > 1e-20){
00293                         actual_measurement = addElementToVector(actual_measurement, meas->twist.angular.z);
00294                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[35]);
00295                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00296                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.angular.z);
00297                         }
00298                 }
00299                 // Linear Acceleration
00300                 if(meas->accel_covariance[0] > 1e-20 && meas->accel_covariance[4] > 1e-20 && meas->accel_covariance[8] > 1e-20){
00301                         geometry_msgs::Vector3 accel_meas_norm = normalized_acceleration(meas->accel);
00302                         actual_measurement = addElementToVector(actual_measurement, accel_meas_norm.x);
00303                         actual_measurement = addElementToVector(actual_measurement, accel_meas_norm.y);
00304                         actual_measurement = addElementToVector(actual_measurement, accel_meas_norm.z);
00305                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->accel_covariance[0]);
00306                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->accel_covariance[4]);
00307                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->accel_covariance[8]);
00308                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00309                                 geometry_msgs::Vector3 res_meas_norm = normalized_acceleration(residuals_msgs[j]->accel);
00310                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], res_meas_norm.x);
00311                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], res_meas_norm.y);
00312                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], res_meas_norm.z);
00313                         }
00314                 }
00315         }
00316 
00317         // Convert covariance vector to matrix
00318         output_innovation_covariance = innovation_covariance_diagonal.asDiagonal();
00319         
00320         return actual_measurement;
00321 }
00322 
00323 void clearMessages(std::vector<boost::shared_ptr<GraftSensor> >& topics){
00324         for(size_t i = 0; i < topics.size(); i++){
00325                 topics[i]->clearMessage();
00326         }
00327 }
00328 
00329 double GraftUKFAttitude::predictAndUpdate(){
00330         if(topics_.size() == 0 || topics_[0] == NULL){
00331                 return 0;
00332         }
00333         ros::Time t = ros::Time::now();
00334         double dt = (t - last_update_time_).toSec();
00335         if(last_update_time_.toSec() < 0.0001){ // No previous updates
00336                 ROS_WARN("Negative dt, skipping update.");
00337                 last_update_time_ = t;
00338                 return 0.0;
00339         }
00340         last_update_time_ = t;
00341 
00342         // Prediction
00343         double lambda = alpha_*alpha_*(SIZE + kappa_) - SIZE;
00344         std::vector<MatrixXd > previous_sigma_points = generateSigmaPoints(graft_state_, graft_covariance_, lambda);
00345         std::vector<MatrixXd > predicted_sigma_points = predict_sigma_points(previous_sigma_points, dt);
00346         MatrixXd predicted_mean = meanFromSigmaPoints(predicted_sigma_points, graft_state_.rows(), lambda);
00347         MatrixXd predicted_covariance = covarianceFromSigmaPoints(predicted_sigma_points, predicted_mean, Q_, graft_state_.rows(), alpha_, beta_, lambda);
00348 
00349         // Update
00350         std::vector<MatrixXd> observation_sigma_points = generateSigmaPoints(predicted_mean, predicted_covariance, lambda);
00351         std::vector<MatrixXd> predicted_observation_sigma_points;
00352         MatrixXd measurement_noise;
00353         MatrixXd z = getMeasurements(topics_, observation_sigma_points, predicted_observation_sigma_points, measurement_noise);
00354         if(z.size() == 0){ // No measurements
00355                 return 0.0;
00356         }
00357         MatrixXd predicted_measurement = meanFromSigmaPoints(predicted_observation_sigma_points, graft_state_.rows(), lambda);
00358         MatrixXd predicted_measurement_uncertainty = covarianceFromSigmaPoints(predicted_observation_sigma_points, predicted_measurement, measurement_noise, graft_state_.rows(), alpha_, beta_, lambda);
00359         MatrixXd cross_covariance = crossCovariance(observation_sigma_points, predicted_mean, predicted_observation_sigma_points, predicted_measurement, alpha_, beta_, lambda);
00360         MatrixXd K = cross_covariance * predicted_measurement_uncertainty.partialPivLu().inverse();
00361         graft_state_ = predicted_mean + K*(z - predicted_measurement);
00362         graft_state_.block(0, 0, 4, 1) = unitQuaternion(graft_state_.block(0, 0, 4, 1));
00363         graft_covariance_ = predicted_covariance - K*predicted_measurement_uncertainty*K.transpose();
00364 
00365         clearMessages(topics_);
00366         return dt;
00367 }
00368 
00369 void GraftUKFAttitude::setTopics(std::vector<boost::shared_ptr<GraftSensor> >& topics){
00370         topics_ = topics;
00371 }
00372 
00373 void GraftUKFAttitude::setInitialCovariance(std::vector<double>& P){
00374         graft_covariance_.setZero();
00375         size_t diagonal_size = std::sqrt(graft_covariance_.size());
00376         if(P.size() == graft_covariance_.size()){ // Full matrix
00377                 for(size_t i = 0; i < P.size(); i++){
00378                         graft_covariance_(i) = P[i];
00379                 }
00380         } else if(P.size() == diagonal_size){ // Diagonal matrix
00381                 for(size_t i = 0; i < P.size(); i++){
00382                         graft_covariance_(i*(diagonal_size+1)) = P[i];
00383                 }
00384         } else { // Not specified correctly
00385                 ROS_ERROR("initial_covariance is size %zu, expected %zu.\nUsing Identity.\nThis probably won't work well.", P.size(), graft_covariance_.size());
00386                 graft_covariance_.setIdentity();
00387         }
00388         std::cout << "cov:\n" << graft_covariance_ << std::endl;
00389 }
00390 
00391 void GraftUKFAttitude::setProcessNoise(std::vector<double>& Q){
00392         Q_.setZero();
00393         size_t diagonal_size = std::sqrt(Q_.size());
00394         if(Q.size() == Q_.size()){ // Full process nosie matrix
00395                 for(size_t i = 0; i < Q.size(); i++){
00396                         Q_(i) = Q[i];
00397                 }
00398         } else if(Q.size() == diagonal_size){ // Diagonal matrix
00399                 for(size_t i = 0; i < Q.size(); i++){
00400                         Q_(i*(diagonal_size+1)) = Q[i];
00401                 }
00402         } else { // Not specified correctly
00403                 ROS_ERROR("Process noise parameter 'Q' is size %zu, expected %zu.\nUsing 0.1*Identity.\nThis probably won't work well.", Q.size(), Q_.size());
00404                 Q_.setIdentity();
00405                 Q_ = 0.1 * Q_;
00406         }
00407 }
00408 
00409 void GraftUKFAttitude::setAlpha(const double alpha){
00410         alpha_ = alpha;
00411 }
00412 
00413 void GraftUKFAttitude::setKappa(const double kappa){
00414         kappa_ = kappa;
00415 }
00416 
00417 void GraftUKFAttitude::setBeta(const double beta){
00418         beta_ = beta;
00419 }


graft
Author(s): Chad Rockey
autogenerated on Sat Jun 8 2019 10:26:16