00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
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
00067 out.push_back(state);
00068
00069
00070 for(size_t i = 1; i <= state.rows(); i++){
00071 out.push_back(state + sig_sqrt.col(i-1));
00072 }
00073
00074
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
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
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
00186 for(size_t i = 0; i < topics.size(); i++){
00187
00188 graft::GraftSensorResidual::ConstPtr meas = topics[i]->z();
00189
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
00195 if(meas == NULL){
00196 continue;
00197 }
00198
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
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
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
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){
00243 ROS_WARN("Negative dt - odom");
00244 last_update_time_ = t;
00245 return 0.0;
00246 }
00247 last_update_time_ = t;
00248
00249
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
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){
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()){
00283 for(size_t i = 0; i < P.size(); i++){
00284 graft_covariance_(i) = P[i];
00285 }
00286 } else if(P.size() == diagonal_size){
00287 for(size_t i = 0; i < P.size(); i++){
00288 graft_covariance_(i*(diagonal_size+1)) = P[i];
00289 }
00290 } else {
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()){
00301 for(size_t i = 0; i < Q.size(); i++){
00302 Q_(i) = Q[i];
00303 }
00304 } else if(Q.size() == diagonal_size){
00305 for(size_t i = 0; i < Q.size(); i++){
00306 Q_(i*(diagonal_size+1)) = Q[i];
00307 }
00308 } else {
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 }