GraftUKFVelocity.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/GraftUKFVelocity.h>
00035  #include <ros/console.h>
00036 
00037  GraftUKFVelocity::GraftUKFVelocity(){
00038         graft_state_.setZero();
00039         graft_control_.setZero();
00040         graft_covariance_.setIdentity();
00041         Q_.setZero();
00042  }
00043 
00044 GraftUKFVelocity::~GraftUKFVelocity(){
00045 
00046 }
00047 
00048 MatrixXd verticalConcatenate(MatrixXd& m, MatrixXd& n){
00049         MatrixXd out;
00050         out.resize(m.rows()+n.rows(), m.cols());
00051         out << m,n;
00052         return out;
00053 }
00054 
00055 MatrixXd matrixSqrt(MatrixXd matrix){ 
00056         // Use LLT Cholesky decomposiion to create stable Matrix Sqrt
00057         return Eigen::LLT<Eigen::MatrixXd>(matrix).matrixL();
00058 }
00059 
00060 std::vector<MatrixXd > generateSigmaPoints(MatrixXd state, MatrixXd covariance, double lambda){
00061         std::vector<MatrixXd > out;
00062 
00063         double gamma = std::sqrt((state.rows()+lambda));
00064         MatrixXd sig_sqrt = gamma*matrixSqrt(covariance);
00065 
00066         // i = 0, push back state as is
00067         out.push_back(state);
00068 
00069         // i = 1,...,n
00070         for(size_t i = 1; i <= state.rows(); i++){
00071                 out.push_back(state + sig_sqrt.col(i-1));
00072         }
00073 
00074         // i = n + 1,...,2n
00075         for(size_t i = state.rows() + 1; i <= 2*state.rows(); i++){
00076                 out.push_back(state - sig_sqrt.col(i-(state.rows()+1)));
00077         }
00078         return out;
00079 }
00080 
00081 MatrixXd meanFromSigmaPoints(std::vector<MatrixXd >& sigma_points, double n, double lambda){
00082         double weight_zero = lambda / (n + lambda);
00083         MatrixXd out = weight_zero * sigma_points[0];
00084         double weight_i = 1.0/(2*(n + lambda));
00085         for(size_t i = 1; i <= 2*n; i++){
00086                 out = out + weight_i * sigma_points[i];
00087         }
00088         return out;
00089 }
00090 
00092 MatrixXd covarianceFromSigmaPoints(std::vector<MatrixXd >& sigma_points, MatrixXd& mean, MatrixXd process_noise, double n, double alpha, double beta, double lambda){
00093         double cov_weight_zero = lambda / (n + lambda) + (1 - alpha*alpha + beta);
00094         MatrixXd out = cov_weight_zero * (sigma_points[0] - mean) * (sigma_points[0] - mean).transpose();
00095         double weight_i = 1.0/(2*(n + lambda));
00096         for(size_t i = 1; i <= 2*n; i++){
00097                 out = out + weight_i  * (sigma_points[i] - mean) * (sigma_points[i] - mean).transpose();
00098         }
00099         return out+process_noise;
00100 }
00101 
00102 MatrixXd crossCovariance(std::vector<MatrixXd >& sigma_points, MatrixXd& mean, std::vector<MatrixXd >& meas_sigma_points, MatrixXd& meas_mean, double alpha, double beta, double lambda){
00103         double n = sigma_points[0].rows();
00104         double cov_weight_zero = lambda / (n + lambda) + (1 - alpha*alpha + beta);
00105         MatrixXd out = cov_weight_zero * (sigma_points[0] - mean) * (meas_sigma_points[0] - meas_mean).transpose();
00106         double weight_i = 1.0/(2*(n + lambda));
00107         for(size_t i = 1; i <= 2*n; i++){
00108                 out = out + weight_i  * (sigma_points[i] - mean) * (meas_sigma_points[i] - meas_mean).transpose();
00109         }
00110         return out;
00111 }
00112 
00113 
00114 MatrixXd GraftUKFVelocity::f(MatrixXd x, double dt){
00115         Matrix<double, SIZE, 1> out;
00116         out.setZero();
00117         out(0) = x(0);
00118         out(1) = x(1);
00119         return out;
00120 }
00121 
00122 graft::GraftState::ConstPtr stateMsgFromMatrix(const MatrixXd& state){
00123         graft::GraftState::Ptr out(new graft::GraftState());
00124         out->twist.linear.x = state(0);
00125         out->twist.linear.y = state(1);
00126         out->twist.angular.z = state(2);
00127         return out;
00128 }
00129 
00130 std::vector<MatrixXd > GraftUKFVelocity::predict_sigma_points(std::vector<MatrixXd >& sigma_points, double dt){
00131         std::vector<MatrixXd > out;
00132         for(size_t i = 0; i < sigma_points.size(); i++){
00133                 out.push_back(f(sigma_points[i], dt));
00134         }
00135         return out;
00136 }
00137 
00138 graft::GraftStatePtr GraftUKFVelocity::getMessageFromState(){
00139         return GraftUKFVelocity::getMessageFromState(graft_state_, graft_covariance_);
00140 }
00141 
00142 graft::GraftStatePtr GraftUKFVelocity::getMessageFromState(Matrix<double, SIZE, 1>& state, Matrix<double, SIZE, SIZE>& covariance){
00143         graft::GraftStatePtr msg(new graft::GraftState());
00144         msg->twist.linear.x = state(0);
00145         msg->twist.linear.y = state(1);
00146         msg->twist.angular.z = state(2);
00147 
00148         for(size_t i = 0; i < SIZE*SIZE; i++){
00149                 msg->covariance[i] = covariance(i);
00150         }
00151         return msg;
00152 }
00153 
00154 VectorXd addElementToVector(const VectorXd& vec, const double& element){
00155         VectorXd out(vec.size() + 1);
00156         out << vec, element;
00157         return out;
00158 }
00159 
00160 MatrixXd addElementToColumnMatrix(const MatrixXd& mat, const double& element){
00161         MatrixXd out(mat.rows() + 1, 1);
00162         MatrixXd small(1, 1);
00163         small(0,0) = element;
00164         if(mat.rows() == 0){
00165                 return small;
00166         }
00167         out << mat, small;
00168         return out;
00169 }
00170 
00171 // Returns measurement vector
00172 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){
00173         VectorXd actual_measurement;
00174         output_measurement_sigmas.clear();
00175         VectorXd innovation_covariance_diagonal;
00176         innovation_covariance_diagonal.resize(0);
00177         // Convert the predicted_sigma_points into messages
00178         std::vector<graft::GraftState::ConstPtr> predicted_sigma_msgs;
00179         for(size_t i = 0; i < predicted_sigma_points.size(); i++){
00180                 predicted_sigma_msgs.push_back(stateMsgFromMatrix(predicted_sigma_points[i]));
00181                 output_measurement_sigmas.push_back(MatrixXd());
00182         }
00183 
00184         
00185         // For each topic
00186         for(size_t i = 0; i < topics.size(); i++){
00187                 // Get the measurement msg and covariance
00188                 graft::GraftSensorResidual::ConstPtr meas = topics[i]->z();
00189                 // Get the predicted measurements
00190                 std::vector<graft::GraftSensorResidual::ConstPtr> residuals_msgs;
00191                 for(size_t j = 0; j < predicted_sigma_msgs.size(); j++){
00192                         residuals_msgs.push_back(topics[i]->h(*predicted_sigma_msgs[j]));
00193                 }
00194                 // Assemble outputs for this topic
00195                 if(meas == NULL){ // Timeout or not received or invalid, skip
00196                         continue;
00197                 }
00198                 // Linear Velocity X
00199                 if(meas->twist_covariance[0] > 1e-20){
00200                         actual_measurement = addElementToVector(actual_measurement, meas->twist.linear.x);
00201                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[0]);
00202                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00203                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.linear.x);
00204                         }
00205                 }
00206                 // Linear Velocity Y
00207                 if(meas->twist_covariance[7] > 1e-20){
00208                         actual_measurement = addElementToVector(actual_measurement, meas->twist.linear.y);
00209                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[7]);
00210                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00211                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.linear.y);
00212                         }
00213                 }
00214                 // Angular Velocity Z
00215                 if(meas->twist_covariance[35] > 1e-20){
00216                         actual_measurement = addElementToVector(actual_measurement, meas->twist.angular.z);
00217                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[35]);
00218                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00219                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.angular.z);
00220                         }
00221                 }
00222         }
00223 
00224         // Convert covariance vector to matrix
00225         output_innovation_covariance = innovation_covariance_diagonal.asDiagonal();
00226         
00227         return actual_measurement;
00228 }
00229 
00230 void clearMessages(std::vector<boost::shared_ptr<GraftSensor> >& topics){
00231         for(size_t i = 0; i < topics.size(); i++){
00232                 topics[i]->clearMessage();
00233         }
00234 }
00235 
00236 double GraftUKFVelocity::predictAndUpdate(){
00237         if(topics_.size() == 0 || topics_[0] == NULL){
00238                 return 0;
00239         }
00240         ros::Time t = ros::Time::now();
00241         double dt = (t - last_update_time_).toSec();
00242         if(last_update_time_.toSec() < 0.0001){ // No previous updates
00243                 ROS_WARN("Negative dt - odom");
00244                 last_update_time_ = t;
00245                 return 0.0;
00246         }
00247         last_update_time_ = t;
00248 
00249         // Prediction
00250         double lambda = alpha_*alpha_*(SIZE + kappa_) - SIZE;
00251         std::vector<MatrixXd > previous_sigma_points = generateSigmaPoints(graft_state_, graft_covariance_, lambda);
00252         std::vector<MatrixXd > predicted_sigma_points = predict_sigma_points(previous_sigma_points, 0.0);
00253         MatrixXd predicted_mean = meanFromSigmaPoints(predicted_sigma_points, graft_state_.rows(), lambda);
00254         MatrixXd predicted_covariance = covarianceFromSigmaPoints(predicted_sigma_points, predicted_mean, Q_, graft_state_.rows(), alpha_, beta_, lambda);
00255 
00256         // Update
00257         std::vector<MatrixXd> observation_sigma_points = generateSigmaPoints(predicted_mean, predicted_covariance, lambda);
00258         std::vector<MatrixXd> predicted_observation_sigma_points;
00259         MatrixXd measurement_noise;
00260         MatrixXd z = getMeasurements(topics_, observation_sigma_points, predicted_observation_sigma_points, measurement_noise);
00261         if(z.size() == 0){ // No measurements
00262                 return 0.0;
00263         }
00264         MatrixXd predicted_measurement = meanFromSigmaPoints(predicted_observation_sigma_points, graft_state_.rows(), lambda);
00265         MatrixXd predicted_measurement_uncertainty = covarianceFromSigmaPoints(predicted_observation_sigma_points, predicted_measurement, measurement_noise, graft_state_.rows(), alpha_, beta_, lambda);
00266         MatrixXd cross_covariance = crossCovariance(observation_sigma_points, predicted_mean, predicted_observation_sigma_points, predicted_measurement, alpha_, beta_, lambda);
00267         MatrixXd K = cross_covariance * predicted_measurement_uncertainty.partialPivLu().inverse();
00268         graft_state_ = predicted_mean + K*(z - predicted_measurement);
00269         graft_covariance_ = predicted_covariance - K*predicted_measurement_uncertainty*K.transpose();
00270 
00271         clearMessages(topics_);
00272         return dt;
00273 }
00274 
00275 void GraftUKFVelocity::setTopics(std::vector<boost::shared_ptr<GraftSensor> >& topics){
00276         topics_ = topics;
00277 }
00278 
00279 void GraftUKFVelocity::setInitialCovariance(std::vector<double>& P){
00280         graft_covariance_.setZero();
00281         size_t diagonal_size = std::sqrt(graft_covariance_.size());
00282         if(P.size() == graft_covariance_.size()){ // Full matrix
00283                 for(size_t i = 0; i < P.size(); i++){
00284                         graft_covariance_(i) = P[i];
00285                 }
00286         } else if(P.size() == diagonal_size){ // Diagonal matrix
00287                 for(size_t i = 0; i < P.size(); i++){
00288                         graft_covariance_(i*(diagonal_size+1)) = P[i];
00289                 }
00290         } else { // Not specified correctly
00291                 ROS_ERROR("initial_covariance is size %zu, expected %zu.\nUsing 0.1*Identity.\nThis probably won't work well.", P.size(), graft_covariance_.size());
00292                 graft_covariance_.setIdentity();
00293                 graft_covariance_ = 0.1 * graft_covariance_;
00294         }
00295 }
00296 
00297 void GraftUKFVelocity::setProcessNoise(std::vector<double>& Q){
00298         Q_.setZero();
00299         size_t diagonal_size = std::sqrt(Q_.size());
00300         if(Q.size() == Q_.size()){ // Full process nosie matrix
00301                 for(size_t i = 0; i < Q.size(); i++){
00302                         Q_(i) = Q[i];
00303                 }
00304         } else if(Q.size() == diagonal_size){ // Diagonal matrix
00305                 for(size_t i = 0; i < Q.size(); i++){
00306                         Q_(i*(diagonal_size+1)) = Q[i];
00307                 }
00308         } else { // Not specified correctly
00309                 ROS_ERROR("process_noise parameter is size %zu, expected %zu.\nUsing 0.1*Identity.\nThis probably won't work well.", Q.size(), Q_.size());
00310                 Q_.setIdentity();
00311                 Q_ = 0.1 * Q_;
00312         }
00313 }
00314 
00315 void GraftUKFVelocity::setAlpha(const double alpha){
00316         alpha_ = alpha;
00317 }
00318 
00319 void GraftUKFVelocity::setKappa(const double kappa){
00320         kappa_ = kappa;
00321 }
00322 
00323 void GraftUKFVelocity::setBeta(const double beta){
00324         beta_ = beta;
00325 }


graft
Author(s): Chad Rockey
autogenerated on Fri Aug 28 2015 10:53:16