GraftUKFAbsolute.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/GraftUKFAbsolute.h>
00035  #include <ros/console.h>
00036 
00037  GraftUKFAbsolute::GraftUKFAbsolute(){
00038         graft_state_.setZero();
00039         graft_state_(3) = 1.0; // Normalize quaternion
00040         graft_control_.setZero();
00041         graft_covariance_.setIdentity();
00042         Q_.setZero();
00043  }
00044 
00045 GraftUKFAbsolute::~GraftUKFAbsolute(){
00046 
00047 }
00048 
00049 MatrixXd verticalConcatenate(MatrixXd& m, MatrixXd& n){
00050         MatrixXd out;
00051         out.resize(m.rows()+n.rows(), m.cols());
00052         out << m,n;
00053         return out;
00054 }
00055 
00056 MatrixXd matrixSqrt(MatrixXd matrix){ 
00057         // Use LLT Cholesky decomposiion to create stable Matrix Sqrt
00058         return Eigen::LLT<Eigen::MatrixXd>(matrix).matrixL();
00059 }
00060 
00061 std::vector<MatrixXd > generateSigmaPoints(MatrixXd state, MatrixXd covariance, double lambda){
00062         std::vector<MatrixXd > out;
00063 
00064         double gamma = std::sqrt((state.rows()+lambda));
00065         MatrixXd sig_sqrt = gamma*matrixSqrt(covariance);
00066 
00067         // i = 0, push back state as is
00068         out.push_back(state);
00069 
00070         // i = 1,...,n
00071         for(size_t i = 1; i <= state.rows(); i++){
00072                 out.push_back(state + sig_sqrt.col(i-1));
00073         }
00074 
00075         // i = n + 1,...,2n
00076         for(size_t i = state.rows() + 1; i <= 2*state.rows(); i++){
00077                 out.push_back(state - sig_sqrt.col(i-(state.rows()+1)));
00078         }
00079         return out;
00080 }
00081 
00082 MatrixXd meanFromSigmaPoints(std::vector<MatrixXd >& sigma_points, double n, double lambda){
00083         double weight_zero = lambda / (n + lambda);
00084         MatrixXd out = weight_zero * sigma_points[0];
00085         double weight_i = 1.0/(2*(n + lambda));
00086         for(size_t i = 1; i <= 2*n; i++){
00087                 out = out + weight_i * sigma_points[i];
00088         }
00089         return out;
00090 }
00091 
00093 MatrixXd covarianceFromSigmaPoints(std::vector<MatrixXd >& sigma_points, MatrixXd& mean, MatrixXd process_noise, double n, double alpha, double beta, double lambda){
00094         double cov_weight_zero = lambda / (n + lambda) + (1 - alpha*alpha + beta);
00095         MatrixXd out = cov_weight_zero * (sigma_points[0] - mean) * (sigma_points[0] - mean).transpose();
00096         double weight_i = 1.0/(2*(n + lambda));
00097         for(size_t i = 1; i <= 2*n; i++){
00098                 out = out + weight_i  * (sigma_points[i] - mean) * (sigma_points[i] - mean).transpose();
00099         }
00100         return out+process_noise;
00101 }
00102 
00103 MatrixXd crossCovariance(std::vector<MatrixXd >& sigma_points, MatrixXd& mean, std::vector<MatrixXd >& meas_sigma_points, MatrixXd& meas_mean, double alpha, double beta, double lambda){
00104         double n = sigma_points[0].rows();
00105         double cov_weight_zero = lambda / (n + lambda) + (1 - alpha*alpha + beta);
00106         MatrixXd out = cov_weight_zero * (sigma_points[0] - mean) * (meas_sigma_points[0] - meas_mean).transpose();
00107         double weight_i = 1.0/(2*(n + lambda));
00108         for(size_t i = 1; i <= 2*n; i++){
00109                 out = out + weight_i  * (sigma_points[i] - mean) * (meas_sigma_points[i] - meas_mean).transpose();
00110         }
00111         return out;
00112 }
00113 
00114 Matrix<double, 4, 4> quaternionUpdateMatrix(const double wx, const double wy, const double wz){
00115         Matrix<double, 4, 4> out;
00116         out <<   0,  wx,  wy,  wz,
00117                -wx,   0, -wz,  wy,
00118                -wy,  wz,   0, -wx,
00119                -wz, -wy,  wx,   0;
00120    return out;
00121 }
00122 
00123 Matrix<double, 4, 1> unitQuaternion(const Matrix<double, 4, 1>& q){
00124         double q_mag = std::sqrt(q(0)*q(0) + q(1)*q(1) + q(2)*q(2) + q(3)*q(3));
00125         return q / q_mag;
00126 }
00127 
00128 Matrix<double, 4, 1> updatedQuaternion(const Matrix<double, 4, 1>& q, const double wx, const double wy, const double wz, double dt){
00129         Matrix<double, 4, 1> out;
00130         Matrix<double, 4, 4> I = Matrix<double, 4, 4>::Identity();
00131         double s = 1.0/2.0 * std::sqrt(wx*wx*dt*dt + wy*wy*dt*dt + wz*wz*dt*dt);
00132         double k = 0.0;
00133         double q_mag = std::sqrt(q(0)*q(0) + q(1)*q(1) + q(2)*q(2) + q(3)*q(3));
00134         double err = 1.0 - q_mag*q_mag;
00135         
00136         double correction_factor = 1 - 1.0/2.0*s*s + 1.0/24.0*s*s*s*s + k * dt * err; // Cosine taylor series
00137         MatrixXd correction = I*(correction_factor);
00138         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
00139         Matrix<double, 4, 4> quaterion_update_matrix = quaternionUpdateMatrix(wx, wy, wz);
00140         MatrixXd update = update_factor*quaterion_update_matrix;
00141         out = (correction-update)*q;
00142 
00143 
00144         double out_mag = std::sqrt(out(0)*out(0) + out(1)*out(1) + out(2)*out(2) + out(3)*out(3));
00145         double out_err = 1.0 - out_mag*out_mag;
00146 
00147         return out;
00148 }
00149 
00150 MatrixXd transformVelocitites(const MatrixXd vel, const MatrixXd quaternion){
00151         Matrix<double, 3, 1> out;
00152         geometry_msgs::Quaternion gquat;
00153         gquat.w = quaternion(0);
00154         gquat.x = quaternion(1);
00155         gquat.y = quaternion(2);
00156         gquat.z = quaternion(3);
00157         tf::Quaternion tfq;
00158         tf::quaternionMsgToTF(gquat, tfq);
00159         tf::Transform tft(tfq, tf::Vector3(0, 0, 0));
00160         tf::Vector3 vel_vector(vel(0), vel(1), vel(2));
00161         tf::Vector3 transformed = tft*vel_vector; // .inverse()
00162         
00163         out(0) = transformed.getX();
00164         out(1) = transformed.getY();
00165         out(2) = transformed.getZ();
00166 
00167         return out;
00168 }
00169 
00170 MatrixXd GraftUKFAbsolute::f(MatrixXd x, double dt){
00171         Matrix<double, SIZE, 1> out;
00172         out.setZero();
00173         MatrixXd rotated_linear_velocity = transformVelocitites(x.block(7, 0, 3, 1), x.block(3, 0, 4, 1));
00174         out(0) = x(0)+rotated_linear_velocity(0)*dt; // x + v_absx*dt
00175         out(1) = x(1)+rotated_linear_velocity(1)*dt; // y + v_absy*dt
00176         out(2) = x(2)+rotated_linear_velocity(2)*dt; // z + v_absz*dt
00177         Matrix<double, 4, 1> new_q = updatedQuaternion(x.block(3, 0, 4, 1), x(10), x(11), x(12), dt);
00178         out.block(3, 0, 4, 1) = new_q; // quaternion
00179         out(7) = x(7); // vx
00180         out(8) = x(8); // vy
00181         out(9) = x(9); // vz
00182         out(10) = x(10); // wz
00183         out(11) = x(11); // wy
00184         out(12) = x(12); // wz
00185         return out;
00186 }
00187 
00188 graft::GraftState::ConstPtr stateMsgFromMatrix(const MatrixXd& state){
00189         graft::GraftState::Ptr out(new graft::GraftState());
00190         out->pose.position.x = state(0);
00191         out->pose.position.y = state(1);
00192         out->pose.position.z = state(2);
00193         out->pose.orientation.w = state(3);
00194         out->pose.orientation.x = state(4);
00195         out->pose.orientation.y = state(5);
00196         out->pose.orientation.z = state(6);
00197         out->twist.linear.x = state(7);
00198         out->twist.linear.y = state(8);
00199         out->twist.linear.z = state(9);
00200         out->twist.angular.x = state(10);
00201         out->twist.angular.y = state(11);
00202         out->twist.angular.z = state(12);
00203         return out;
00204 }
00205 
00206 std::vector<MatrixXd > GraftUKFAbsolute::predict_sigma_points(std::vector<MatrixXd >& sigma_points, double dt){
00207         std::vector<MatrixXd > out;
00208         for(size_t i = 0; i < sigma_points.size(); i++){
00209                 out.push_back(f(sigma_points[i], dt));
00210         }
00211         return out;
00212 }
00213 
00214 graft::GraftStatePtr GraftUKFAbsolute::getMessageFromState(){
00215         return GraftUKFAbsolute::getMessageFromState(graft_state_, graft_covariance_);
00216 }
00217 
00218 graft::GraftStatePtr GraftUKFAbsolute::getMessageFromState(Matrix<double, SIZE, 1>& state, Matrix<double, SIZE, SIZE>& covariance){
00219         graft::GraftStatePtr msg(new graft::GraftState());
00220         msg->pose.position.x = state(0);
00221         msg->pose.position.y = state(1);
00222         msg->pose.position.z = state(2);
00223         msg->pose.orientation.w = state(3);
00224         msg->pose.orientation.x = state(4);
00225         msg->pose.orientation.y = state(5);
00226         msg->pose.orientation.z = state(6);
00227         msg->twist.linear.x = state(7);
00228         msg->twist.linear.y = state(8);
00229         msg->twist.linear.z = state(9);
00230         msg->twist.angular.x = state(10);
00231         msg->twist.angular.y = state(11);
00232         msg->twist.angular.z = state(12);
00233 
00234         for(size_t i = 0; i < SIZE*SIZE; i++){
00235                 msg->covariance[i] = covariance(i);
00236         }
00237         return msg;
00238 }
00239 
00240 VectorXd addElementToVector(const VectorXd& vec, const double& element){
00241         VectorXd out(vec.size() + 1);
00242         out << vec, element;
00243         return out;
00244 }
00245 
00246 MatrixXd addElementToColumnMatrix(const MatrixXd& mat, const double& element){
00247         MatrixXd out(mat.rows() + 1, 1);
00248         MatrixXd small(1, 1);
00249         small(0,0) = element;
00250         if(mat.rows() == 0){
00251                 return small;
00252         }
00253         out << mat, small;
00254         return out;
00255 }
00256 
00257 // Returns measurement vector
00258 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){
00259         VectorXd actual_measurement;
00260         output_measurement_sigmas.clear();
00261         VectorXd innovation_covariance_diagonal;
00262         innovation_covariance_diagonal.resize(0);
00263         // Convert the predicted_sigma_points into messages
00264         std::vector<graft::GraftState::ConstPtr> predicted_sigma_msgs;
00265         for(size_t i = 0; i < predicted_sigma_points.size(); i++){
00266                 predicted_sigma_msgs.push_back(stateMsgFromMatrix(predicted_sigma_points[i]));
00267                 output_measurement_sigmas.push_back(MatrixXd());
00268         }
00269 
00270         
00271         // For each topic
00272         for(size_t i = 0; i < topics.size(); i++){
00273                 // Get the measurement msg and covariance
00274                 graft::GraftSensorResidual::ConstPtr meas = topics[i]->z();
00275                 // Get the predicted measurements
00276                 std::vector<graft::GraftSensorResidual::ConstPtr> residuals_msgs;
00277                 for(size_t j = 0; j < predicted_sigma_msgs.size(); j++){
00278                         residuals_msgs.push_back(topics[i]->h(*predicted_sigma_msgs[j]));
00279                 }
00280                 // Assemble outputs for this topic
00281                 if(meas == NULL){ // Timeout or not received or invalid, skip
00282                         continue;
00283                 }
00284                 // Position X
00285                 if(meas->pose_covariance[0] > 1e-20){
00286                         actual_measurement = addElementToVector(actual_measurement, meas->pose.position.x);
00287                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->pose_covariance[0]);
00288                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00289                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->pose.position.x);
00290                         }
00291                 }
00292                 // Position Y
00293                 if(meas->pose_covariance[7] > 1e-20){
00294                         actual_measurement = addElementToVector(actual_measurement, meas->pose.position.y);
00295                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->pose_covariance[7]);
00296                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00297                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->pose.position.y);
00298                         }
00299                 }
00300                 // Position Z
00301                 if(meas->pose_covariance[14] > 1e-20){
00302                         actual_measurement = addElementToVector(actual_measurement, meas->pose.position.z);
00303                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->pose_covariance[14]);
00304                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00305                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->pose.position.z);
00306                         }
00307                 }
00308                 // Linear Velocity X
00309                 if(meas->twist_covariance[0] > 1e-20){
00310                         actual_measurement = addElementToVector(actual_measurement, meas->twist.linear.x);
00311                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[0]);
00312                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00313                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.linear.x);
00314                         }
00315                 }
00316                 // Linear Velocity Y
00317                 if(meas->twist_covariance[7] > 1e-20){
00318                         actual_measurement = addElementToVector(actual_measurement, meas->twist.linear.y);
00319                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[7]);
00320                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00321                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.linear.y);
00322                         }
00323                 }
00324                 // Linear Velocity Z
00325                 if(meas->twist_covariance[14] > 1e-20){
00326                         actual_measurement = addElementToVector(actual_measurement, meas->twist.linear.z);
00327                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[14]);
00328                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00329                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.linear.z);
00330                         }
00331                 }
00332                 // Angular Velocity X
00333                 if(meas->twist_covariance[21] > 1e-20){
00334                         actual_measurement = addElementToVector(actual_measurement, meas->twist.angular.x);
00335                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[21]);
00336                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00337                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.angular.x);
00338                         }
00339                 }
00340                 // Angular Velocity Y
00341                 if(meas->twist_covariance[28] > 1e-20){
00342                         actual_measurement = addElementToVector(actual_measurement, meas->twist.angular.y);
00343                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[28]);
00344                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00345                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.angular.y);
00346                         }
00347                 }
00348                 // Angular Velocity Z
00349                 if(meas->twist_covariance[35] > 1e-20){
00350                         actual_measurement = addElementToVector(actual_measurement, meas->twist.angular.z);
00351                         innovation_covariance_diagonal = addElementToVector(innovation_covariance_diagonal, meas->twist_covariance[35]);
00352                         for(size_t j = 0; j < residuals_msgs.size(); j++){
00353                                 output_measurement_sigmas[j] = addElementToColumnMatrix(output_measurement_sigmas[j], residuals_msgs[j]->twist.angular.z);
00354                         }
00355                 }
00356         }
00357 
00358         // Convert covariance vector to matrix
00359         output_innovation_covariance = innovation_covariance_diagonal.asDiagonal();
00360         
00361         return actual_measurement;
00362 }
00363 
00364 void clearMessages(std::vector<boost::shared_ptr<GraftSensor> >& topics){
00365         for(size_t i = 0; i < topics.size(); i++){
00366                 topics[i]->clearMessage();
00367         }
00368 }
00369 
00370 double GraftUKFAbsolute::predictAndUpdate(){
00371         if(topics_.size() == 0 || topics_[0] == NULL){
00372                 return 0;
00373         }
00374         ros::Time t = ros::Time::now();
00375         double dt = (t - last_update_time_).toSec();
00376         if(last_update_time_.toSec() < 0.0001){ // No previous updates
00377                 ROS_WARN("Negative dt - odom");
00378                 last_update_time_ = t;
00379                 return 0.0;
00380         }
00381         last_update_time_ = t;
00382 
00383         // Prediction
00384         double lambda = alpha_*alpha_*(SIZE + kappa_) - SIZE;
00385         std::vector<MatrixXd > previous_sigma_points = generateSigmaPoints(graft_state_, graft_covariance_, lambda);
00386         std::vector<MatrixXd > predicted_sigma_points = predict_sigma_points(previous_sigma_points, dt);
00387         MatrixXd predicted_mean = meanFromSigmaPoints(predicted_sigma_points, graft_state_.rows(), lambda);
00388         MatrixXd predicted_covariance = covarianceFromSigmaPoints(predicted_sigma_points, predicted_mean, Q_, graft_state_.rows(), alpha_, beta_, lambda);
00389 
00390         // Update
00391         std::vector<MatrixXd> observation_sigma_points = generateSigmaPoints(predicted_mean, predicted_covariance, lambda);
00392         std::vector<MatrixXd> predicted_observation_sigma_points;
00393         MatrixXd measurement_noise;
00394         MatrixXd z = getMeasurements(topics_, observation_sigma_points, predicted_observation_sigma_points, measurement_noise);
00395         if(z.size() == 0){ // No measurements
00396                 return 0.0;
00397         }
00398         MatrixXd predicted_measurement = meanFromSigmaPoints(predicted_observation_sigma_points, graft_state_.rows(), lambda);
00399         MatrixXd predicted_measurement_uncertainty = covarianceFromSigmaPoints(predicted_observation_sigma_points, predicted_measurement, measurement_noise, graft_state_.rows(), alpha_, beta_, lambda);
00400         MatrixXd cross_covariance = crossCovariance(observation_sigma_points, predicted_mean, predicted_observation_sigma_points, predicted_measurement, alpha_, beta_, lambda);
00401         MatrixXd K = cross_covariance * predicted_measurement_uncertainty.partialPivLu().inverse();
00402         graft_state_ = predicted_mean + K*(z - predicted_measurement);
00403         graft_state_.block(3, 0, 4, 1) = unitQuaternion(graft_state_.block(3, 0, 4, 1));
00404 
00405         graft_covariance_ = predicted_covariance - K*predicted_measurement_uncertainty*K.transpose();
00406 
00407         clearMessages(topics_);
00408         return dt;
00409 }
00410 
00411 void GraftUKFAbsolute::setTopics(std::vector<boost::shared_ptr<GraftSensor> >& topics){
00412         topics_ = topics;
00413 }
00414 
00415 void GraftUKFAbsolute::setInitialCovariance(std::vector<double>& P){
00416         graft_covariance_.setZero();
00417         size_t diagonal_size = std::sqrt(graft_covariance_.size());
00418         if(P.size() == graft_covariance_.size()){ // Full matrix
00419                 for(size_t i = 0; i < P.size(); i++){
00420                         graft_covariance_(i) = P[i];
00421                 }
00422         } else if(P.size() == diagonal_size){ // Diagonal matrix
00423                 for(size_t i = 0; i < P.size(); i++){
00424                         graft_covariance_(i*(diagonal_size+1)) = P[i];
00425                 }
00426         } else { // Not specified correctly
00427                 ROS_ERROR("initial_covariance is size %zu, expected %zu.\nUsing 0.1*Identity.\nThis probably won't work well.", P.size(), graft_covariance_.size());
00428                 graft_covariance_.setIdentity();
00429                 graft_covariance_ = 0.1 * graft_covariance_;
00430         }
00431 }
00432 
00433 void GraftUKFAbsolute::setProcessNoise(std::vector<double>& Q){
00434         Q_.setZero();
00435         size_t diagonal_size = std::sqrt(Q_.size());
00436         if(Q.size() == Q_.size()){ // Full process nosie matrix
00437                 for(size_t i = 0; i < Q.size(); i++){
00438                         Q_(i) = Q[i];
00439                 }
00440         } else if(Q.size() == diagonal_size){ // Diagonal matrix
00441                 for(size_t i = 0; i < Q.size(); i++){
00442                         Q_(i*(diagonal_size+1)) = Q[i];
00443                 }
00444         } else { // Not specified correctly
00445                 ROS_ERROR("process_noise parameter is size %zu, expected %zu.\nUsing 0.1*Identity.\nThis probably won't work well.", Q.size(), Q_.size());
00446                 Q_.setIdentity();
00447                 Q_ = 0.1 * Q_;
00448         }
00449 }
00450 
00451 void GraftUKFAbsolute::setAlpha(const double alpha){
00452         alpha_ = alpha;
00453 }
00454 
00455 void GraftUKFAbsolute::setKappa(const double kappa){
00456         kappa_ = kappa;
00457 }
00458 
00459 void GraftUKFAbsolute::setBeta(const double beta){
00460         beta_ = beta;
00461 }


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