static int find_affine(int np, double *pts1, double *pts2, double *mat) { const int np2 = np * 2; double *a = (double *)aom_malloc(sizeof(*a) * np2 * 13); double *b = a + np2 * 6; double *temp = b + np2; int i; double sx, sy, dx, dy; double T1[9], T2[9]; normalize_homography(pts1, np, T1); normalize_homography(pts2, np, T2); for (i = 0; i < np; ++i) { dx = *(pts2++); dy = *(pts2++); sx = *(pts1++); sy = *(pts1++); a[i * 2 * 6 + 0] = sx; a[i * 2 * 6 + 1] = sy; a[i * 2 * 6 + 2] = 0; a[i * 2 * 6 + 3] = 0; a[i * 2 * 6 + 4] = 1; a[i * 2 * 6 + 5] = 0; a[(i * 2 + 1) * 6 + 0] = 0; a[(i * 2 + 1) * 6 + 1] = 0; a[(i * 2 + 1) * 6 + 2] = sx; a[(i * 2 + 1) * 6 + 3] = sy; a[(i * 2 + 1) * 6 + 4] = 0; a[(i * 2 + 1) * 6 + 5] = 1; b[2 * i] = dx; b[2 * i + 1] = dy; } if (pseudo_inverse(temp, a, np2, 6)) { aom_free(a); return 1; } multiply_mat(temp, b, mat, 6, np2, 1); denormalize_affine_reorder(mat, T1, T2); aom_free(a); return 0; }
void TaskInverseKinematics::update(const ros::Time& time, const ros::Duration& period) { // get joint positions for(int i=0; i < joint_handles_.size(); i++) { joint_msr_states_.q(i) = joint_handles_[i].getPosition(); joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity(); } if (cmd_flag_) { // computing Jacobian jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_); // computing J_pinv_ pseudo_inverse(J_.data,J_pinv_); // computing forward kinematics fk_pos_solver_->JntToCart(joint_msr_states_.q,x_); // end-effector position/orientation error x_err_.vel = x_des_.p - x_.p; // getting quaternion from rotation matrix x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a); x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a); skew_symmetric(quat_des_.v,skew_); for (int i = 0; i < skew_.rows(); i++) { v_temp_(i) = 0.0; for (int k = 0; k < skew_.cols(); k++) v_temp_(i) += skew_(i,k)*(quat_curr_.v(k)); } x_err_.rot = quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v - v_temp_; // computing q_dot for (int i = 0; i < J_pinv_.rows(); i++) { joint_des_states_.qdot(i) = 0.0; for (int k = 0; k < J_pinv_.cols(); k++) joint_des_states_.qdot(i) += J_pinv_(i,k)*x_err_(k); //removed scaling factor of .7 } // integrating q_dot -> getting q (Euler method) for (int i = 0; i < joint_handles_.size(); i++) joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i); // joint limits saturation for (int i =0; i < joint_handles_.size(); i++) { if (joint_des_states_.q(i) < joint_limits_.min(i)) joint_des_states_.q(i) = joint_limits_.min(i); if (joint_des_states_.q(i) > joint_limits_.max(i)) joint_des_states_.q(i) = joint_limits_.max(i); } if (Equal(x_,x_des_,0.005)) { ROS_INFO("On target"); cmd_flag_ = 0; } } // set controls for joints for (int i = 0; i < joint_handles_.size(); i++) { tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period); joint_handles_[i].setCommand(tau_cmd_(i)); } }
void DynamicSlidingModeControllerTaskSpace::update(const ros::Time& time, const ros::Duration& period) { // get joint positions for(int i=0; i < joint_handles_.size(); i++) { joint_msr_states_.q(i) = joint_handles_[i].getPosition(); joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity(); } if (cmd_flag_) { // computing forward kinematics fk_pos_solver_->JntToCart(joint_msr_states_.q,x_); // computing Jacobian J(q) jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_); // computing Jacobian pseudo-inversion pseudo_inverse(J_.data,J_pinv_); // computing end-effector position/orientation error w.r.t. desired frame x_err_ = diff(x_,x_des_); /* Trying quaternions, it seems to work better // end-effector position/orientation error x_err_.vel = (x_des_.p - x_.p); // getting quaternion from rotation matrix x_.M.GetQuaternion(quat_curr_.v(0),quat_curr_.v(1),quat_curr_.v(2),quat_curr_.a); x_des_.M.GetQuaternion(quat_des_.v(0),quat_des_.v(1),quat_des_.v(2),quat_des_.a); skew_symmetric(quat_des_.v,skew_); for (int i = 0; i < skew_.rows(); i++) { v_temp_(i) = 0.0; for (int k = 0; k < skew_.cols(); k++) v_temp_(i) += skew_(i,k)*(quat_curr_.v(k)); } x_err_.rot = (quat_curr_.a*quat_des_.v - quat_des_.a*quat_curr_.v) - v_temp_; */ // clearing error msg before publishing msg_err_.data.clear(); for(int i = 0; i < e_ref_.size(); i++) { e_ref_(i) = x_err_(i); msg_err_.data.push_back(e_ref_(i)); } joint_des_states_.qdot.data = J_pinv_*e_ref_; joint_des_states_.q.data = joint_msr_states_.q.data + period.toSec()*joint_des_states_.qdot.data; // computing S S_.data = (joint_msr_states_.qdot.data - joint_des_states_.qdot.data) + alpha_.data.cwiseProduct(joint_msr_states_.q.data - joint_des_states_.q.data); //for (int i = 0; i < joint_handles_.size(); i++) //S_(i) = (joint_msr_states_.qdot(i) - joint_des_states_.qdot(i)) + alpha_(i)*tanh(lambda_(i)*(joint_msr_states_.q(i) - joint_des_states_.q(i))); // saving S0 on the first step if (step_ == 0) S0_ = S_; // computing Sd for (int i = 0; i < joint_handles_.size(); i++) Sd_(i) = S0_(i)*exp(-k_(i)*(step_*period.toSec())); Sq_.data = S_.data + Sd_.data;//- Sd_.data; // computing sigma_dot as sgn(Sq) for (int i = 0; i < joint_handles_.size(); i++) sigma_dot_(i) = -(Sq_(i) < 0) + (Sq_(i) > 0); // integrating sigma_dot sigma_.data += period.toSec()*sigma_dot_.data; //for (int i = 0; i < joint_handles_.size(); i++) //sigma_(i) += period.toSec()*pow(Sq_(i),0.5); // computing Sr Sr_.data = Sq_.data + gamma_.data.cwiseProduct(sigma_.data); // computing tau tau_.data = -Kd_.data.cwiseProduct(Sr_.data); step_++; if (Equal(x_,x_des_,0.05)) { ROS_INFO("On target"); cmd_flag_ = 0; return; } } // set controls for joints for (int i = 0; i < joint_handles_.size(); i++) { if (!cmd_flag_) tau_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period);//Kd_(i)*(alpha_(i)*(joint_des_states_.q(i) - joint_msr_states_.q(i)) + (joint_des_states_.qdot(i) - joint_msr_states_.qdot(i))) ; joint_handles_[i].setCommand(tau_(i)); } // publishing markers for visualization in rviz pub_marker_.publish(msg_marker_); msg_id_++; // publishing error for all tasks as an array of ntasks*6 pub_error_.publish(msg_err_); // publishing actual and desired trajectory for each task (links) as an array of ntasks*3 pub_pose_.publish(msg_pose_); pub_traj_.publish(msg_traj_); ros::spinOnce(); }
void MultiTaskPriorityInverseKinematics::update(const ros::Time& time, const ros::Duration& period) { // get joint positions for(int i=0; i < joint_handles_.size(); i++) { joint_msr_states_.q(i) = joint_handles_[i].getPosition(); joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity(); } // clearing error msg before publishing msg_err_.data.clear(); if (cmd_flag_) { // resetting P and qdot(t=0) for the highest priority task P_ = I_; SetToZero(joint_des_states_.qdot); for (int index = 0; index < ntasks_; index++) { // computing Jacobian jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_,links_index_[index]); // computing forward kinematics fk_pos_solver_->JntToCart(joint_msr_states_.q,x_,links_index_[index]); // setting marker parameters set_marker(x_,index,msg_id_); // computing end-effector position/orientation error w.r.t. desired frame x_err_ = diff(x_,x_des_[index]); for(int i = 0; i < e_dot_.size(); i++) { e_dot_(i) = x_err_(i); msg_err_.data.push_back(e_dot_(i)); } // computing (J[i]*P[i-1])^pinv J_star_.data = J_.data*P_; pseudo_inverse(J_star_.data,J_pinv_); // computing q_dot (qdot(i) = qdot[i-1] + (J[i]*P[i-1])^pinv*(x_err[i] - J[i]*qdot[i-1])) joint_des_states_.qdot.data = joint_des_states_.qdot.data + J_pinv_*(e_dot_ - J_.data*joint_des_states_.qdot.data); // stop condition if (!on_target_flag_[index]) { if (Equal(x_,x_des_[index],0.01)) { ROS_INFO("Task %d on target",index); on_target_flag_[index] = true; if (index == (ntasks_ - 1)) cmd_flag_ = 0; } } // updating P_ (it mustn't make use of the damped pseudo inverse) pseudo_inverse(J_star_.data,J_pinv_,false); P_ = P_ - J_pinv_*J_star_.data; } // integrating q_dot -> getting q (Euler method) for (int i = 0; i < joint_handles_.size(); i++) joint_des_states_.q(i) += period.toSec()*joint_des_states_.qdot(i); } // set controls for joints for (int i = 0; i < joint_handles_.size(); i++) { tau_cmd_(i) = PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),joint_des_states_.qdot(i) - joint_msr_states_.qdot(i),period); joint_handles_[i].setCommand(tau_cmd_(i)); } // publishing markers for visualization in rviz pub_marker_.publish(msg_marker_); msg_id_++; // publishing error for all tasks as an array of ntasks*6 pub_error_.publish(msg_err_); ros::spinOnce(); }
void BacksteppingController::update(const ros::Time& time, const ros::Duration& period) { // get joint positions for(int i=0; i < joint_handles_.size(); i++) { joint_msr_states_.q(i) = joint_handles_[i].getPosition(); joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity(); //joint_des_states_.q(i) = joint_msr_states_.q(i); //joint_des_states_.qdot(i) = joint_msr_states_.qdot(i); } /* TASK 0 (Main)*/ double freq_ = 3.0; double amp_ = 0.1; //Desired posture 8 figure (circle figure) x_des_.p(0) = /*0.3*/0.25 + /*amp_*(1 - cos(freq_*period.toSec()*step_));*/amp_/2*sin(freq_*period.toSec()*step_); x_des_.p(1) = /*0.3*/0 + /*amp_*sin(freq_*period.toSec()*step_);*/amp_/2*sin(2*freq_*period.toSec()*step_); x_des_.p(2) = /*1.1*/1.75; x_des_.M = KDL::Rotation(KDL::Rotation::RPY(0,0,0));//RPY(0,3.1415,0)); //velocity x_des_dot_(0) = /*amp_*freq_*sin(freq_*period.toSec()*step_);*/amp_/2*freq_*cos(freq_*period.toSec()*step_); x_des_dot_(1) = /*amp_*freq_*cos(freq_*period.toSec()*step_);*/amp_/2*2*freq_*cos(2*freq_*period.toSec()*step_); x_des_dot_(2) = 0; x_des_dot_(3) = 0; x_des_dot_(4) = 0; x_des_dot_(5) = 0; //acc x_des_dotdot_(0) = /*amp_*freq_*freq_*cos(freq_*period.toSec()*step_);*/-amp_/2*freq_*freq_*sin(freq_*period.toSec()*step_); x_des_dotdot_(1) = /*-amp_*freq_*freq_*sin(freq_*period.toSec()*step_);*/-amp_/2*2*2*freq_*freq_*sin(freq_*period.toSec()*step_); x_des_dotdot_(2) = 0; x_des_dotdot_(3) = 0; x_des_dotdot_(4) = 0; x_des_dotdot_(5) = 0; step_++; // clearing msgs before publishing msg_err_.data.clear(); msg_pose_.data.clear(); msg_traj_.data.clear(); if (cmd_flag_) { // computing Inertia, Coriolis and Gravity matrices id_solver_->JntToMass(joint_msr_states_.q, M_); id_solver_->JntToCoriolis(joint_msr_states_.q, joint_msr_states_.qdot, C_); id_solver_->JntToGravity(joint_msr_states_.q, G_); // computing Jacobian J(q) jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_); // computing Jacobian pseudo-inversion pseudo_inverse(J_.data,J_pinv_); // using the first step to compute jacobian of the tasks if (first_step_) { J_last_ = J_; first_step_ = 0; return; } // computing the derivative of Jacobian J_dot(q) through numerical differentiation J_dot_.data = (J_.data - J_last_.data)/period.toSec(); // computing forward kinematics fk_pos_solver_->JntToCart(joint_msr_states_.q,x_); // pushing x to the pose msg for (int i = 0; i < 3; i++) { msg_pose_.data.push_back(x_.p(i)); msg_traj_.data.push_back(x_des_.p(i)); } // setting marker parameters set_marker(x_,msg_id_); // computing end-effector position/orientation error w.r.t. desired frame x_err_ = diff(x_,x_des_); // computing task space velocity (of end-effector) x_dot_ = J_.data*joint_msr_states_.qdot.data; // computing reference variables in task space for (int i = 0; i < 6; i++) { x_ref_dot_(i) = x_des_dot_(i) + Kp_(i)*x_err_(i); x_ref_dotdot_(i) = x_des_dotdot_(i) + Kd_(i)*(x_des_dot_(i) - x_dot_(i)) + Kp_(i)*x_err_(i); } // computing reference variable in joint space joint_ref_.qdot.data = J_pinv_*x_ref_dot_; joint_ref_.qdotdot.data = J_pinv_*(x_ref_dotdot_ - J_dot_.data*joint_msr_states_.qdot.data); joint_des_states_.qdot.data = J_pinv_*x_des_dot_; joint_des_states_.q.data += period.toSec()*joint_des_states_.qdot.data; // finally, computing the torque tau tau_.data = M_.data*joint_ref_.qdotdot.data + C_.data.cwiseProduct(joint_ref_.qdot.data) + G_.data + /*K_d*/(joint_ref_.qdot.data - joint_msr_states_.qdot.data) + (joint_des_states_.q.data - joint_msr_states_.q.data); // saving J_ of the last iteration J_last_ = J_; } // set controls for joints for (int i = 0; i < joint_handles_.size(); i++) { if(cmd_flag_) joint_handles_[i].setCommand(tau_(i)); else joint_handles_[i].setCommand(PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period)); } // publishing markers for visualization in rviz pub_marker_.publish(msg_marker_); msg_id_++; // publishing error for all tasks as an array of ntasks*6 pub_error_.publish(msg_err_); // publishing actual and desired trajectory for each task (links) as an array of ntasks*3 pub_pose_.publish(msg_pose_); pub_traj_.publish(msg_traj_); ros::spinOnce(); }
void OneTaskInverseDynamicsJL::update(const ros::Time& time, const ros::Duration& period) { // get joint positions for(int i=0; i < joint_handles_.size(); i++) { joint_msr_states_.q(i) = joint_handles_[i].getPosition(); joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity(); } // clearing msgs before publishing msg_err_.data.clear(); msg_pose_.data.clear(); if (cmd_flag_) { // resetting N and tau(t=0) for the highest priority task N_trans_ = I_; SetToZero(tau_); // computing Inertia, Coriolis and Gravity matrices id_solver_->JntToMass(joint_msr_states_.q, M_); id_solver_->JntToCoriolis(joint_msr_states_.q, joint_msr_states_.qdot, C_); id_solver_->JntToGravity(joint_msr_states_.q, G_); G_.data.setZero(); // computing the inverse of M_ now, since it will be used often pseudo_inverse(M_.data,M_inv_,false); //M_inv_ = M_.data.inverse(); // computing Jacobian J(q) jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_); // computing the distance from the mid points of the joint ranges as objective function to be minimized phi_ = task_objective_function(joint_msr_states_.q); // using the first step to compute jacobian of the tasks if (first_step_) { J_last_ = J_; phi_last_ = phi_; first_step_ = 0; return; } // computing the derivative of Jacobian J_dot(q) through numerical differentiation J_dot_.data = (J_.data - J_last_.data)/period.toSec(); // computing forward kinematics fk_pos_solver_->JntToCart(joint_msr_states_.q,x_); if (Equal(x_,x_des_,0.05)) { ROS_INFO("On target"); cmd_flag_ = 0; return; } // pushing x to the pose msg for (int i = 0; i < 3; i++) msg_pose_.data.push_back(x_.p(i)); // setting marker parameters set_marker(x_,msg_id_); // computing end-effector position/orientation error w.r.t. desired frame x_err_ = diff(x_,x_des_); x_dot_ = J_.data*joint_msr_states_.qdot.data; // setting error reference for(int i = 0; i < e_ref_.size(); i++) { // e = x_des_dotdot + Kd*(x_des_dot - x_dot) + Kp*(x_des - x) e_ref_(i) = -Kd_(i)*(x_dot_(i)) + Kp_(i)*x_err_(i); msg_err_.data.push_back(e_ref_(i)); } // computing b = J*M^-1*(c+g) - J_dot*q_dot b_ = J_.data*M_inv_*(C_.data + G_.data) - J_dot_.data*joint_msr_states_.qdot.data; // computing omega = J*M^-1*N^T*J omega_ = J_.data*M_inv_*N_trans_*J_.data.transpose(); // computing lambda = omega^-1 pseudo_inverse(omega_,lambda_); //lambda_ = omega_.inverse(); // computing nullspace N_trans_ = N_trans_ - J_.data.transpose()*lambda_*J_.data*M_inv_; // finally, computing the torque tau tau_.data = J_.data.transpose()*lambda_*(e_ref_ + b_) + N_trans_*(Eigen::Matrix<double,7,1>::Identity(7,1)*(phi_ - phi_last_)/(period.toSec())); // saving J_ and phi of the last iteration J_last_ = J_; phi_last_ = phi_; } // set controls for joints for (int i = 0; i < joint_handles_.size(); i++) { if(cmd_flag_) joint_handles_[i].setCommand(tau_(i)); else joint_handles_[i].setCommand(PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period)); } // publishing markers for visualization in rviz pub_marker_.publish(msg_marker_); msg_id_++; // publishing error pub_error_.publish(msg_err_); // publishing pose pub_pose_.publish(msg_pose_); ros::spinOnce(); }
int main() { srand(time(NULL)); int states = 6; int unfiltered_states = 3; gsl_matrix *state_mean = gsl_matrix_calloc(states,1); gsl_matrix_set(state_mean, 0,0, 3); gsl_matrix_set(state_mean, 1,0, 3); gsl_matrix *state_covariance = gsl_matrix_calloc(states,states); gsl_matrix *observation_mean = gsl_matrix_calloc(states,1); gsl_matrix *observation_covariance = gsl_matrix_calloc(states,states); //gsl_matrix_set(observation_covariance, 0, 0, hdop); //gsl_matrix_set(observation_covariance, 1, 1, hdop); //gsl_matrix_set(observation_covariance, 2, 2, vert_err); //gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2)); //gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2)); gsl_matrix *observation_transformation = gsl_matrix_calloc(states,states); gsl_matrix *estimate_mean = gsl_matrix_calloc(states,1); gsl_matrix *estimate_covariance = gsl_matrix_calloc(states,states); gsl_matrix *kalman_gain = gsl_matrix_calloc(states,states); gsl_matrix *temp21a = gsl_matrix_calloc(states,1); gsl_matrix *temp21b = gsl_matrix_calloc(states,1); gsl_matrix *temp22a = gsl_matrix_calloc(states,states); gsl_matrix *temp22b = gsl_matrix_calloc(states,states); gsl_matrix *predict = gsl_matrix_calloc(states,states); //gsl_matrix_set(predict, 0, 0, 1); //gsl_matrix_set(predict, 1, 1, 1); //gsl_matrix_set(predict, 2, 2, 1); gsl_matrix_set(predict, 3, 3, 1); gsl_matrix_set(predict, 4, 4, 1); gsl_matrix_set(predict, 5, 5, 1); //gsl_matrix_set(predict, 0, 3, timestep); //gsl_matrix_set(predict, 1, 4, timestep); //gsl_matrix_set(predict, 2, 5, timestep); gsl_matrix *control = gsl_matrix_calloc(states, unfiltered_states); //gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2)); //gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2)); //gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2)); //gsl_matrix_set(control, 3, 0, timestep); //gsl_matrix_set(control, 4, 1, timestep); //gsl_matrix_set(control, 5, 2, timestep); gsl_vector *acceleration = gsl_vector_calloc(unfiltered_states); gsl_vector *velocity = gsl_vector_calloc(unfiltered_states); gsl_vector *location = gsl_vector_calloc(unfiltered_states); gsl_vector *delta_location = gsl_vector_calloc(unfiltered_states); gsl_vector *temp_location = gsl_vector_calloc(unfiltered_states); double timestep; int hdop; int vert_err; int s; double error; for (int t = 1; t < 1000000; t++) { timestep = 1; gsl_vector_set(acceleration, 0, (rand()%101)-52); gsl_vector_set(acceleration, 1, (rand()%101)-46); gsl_vector_set(acceleration, 2, (rand()%101)-54); gsl_vector_add(velocity, acceleration); gsl_vector_add(location, velocity); gsl_vector_memcpy(delta_location, location); gsl_vector_sub(delta_location, temp_location); gsl_vector_memcpy(temp_location, location); gsl_matrix_set(predict, 0, 3, timestep); gsl_matrix_set(predict, 1, 4, timestep); gsl_matrix_set(predict, 2, 5, timestep); gsl_matrix_set(control, 0, 0, 0.5*pow(timestep,2)); gsl_matrix_set(control, 1, 1, 0.5*pow(timestep,2)); gsl_matrix_set(control, 2, 2, 0.5*pow(timestep,2)); gsl_matrix_set(control, 3, 0, timestep); gsl_matrix_set(control, 4, 1, timestep); gsl_matrix_set(control, 5, 2, timestep); hdop = 5; vert_err = 5; gsl_matrix_set(observation_covariance, 0, 0, hdop); gsl_matrix_set(observation_covariance, 1, 1, hdop); gsl_matrix_set(observation_covariance, 2, 2, vert_err); gsl_matrix_set(observation_covariance, 3, 3, (2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 4, 4, (2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 5, 5, (2*vert_err)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 0, 3, timestep*(2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 3, 0, timestep*(2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 1, 4, timestep*(2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 4, 1, timestep*(2*hdop)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 2, 5, timestep*(2*vert_err)/pow(timestep,2)); gsl_matrix_set(observation_covariance, 5, 2, timestep*(2*vert_err)/pow(timestep,2)); //observation_transformation = observation_mean[k] * pseudoinverse(state_mean[k-1]) gsl_matrix_set_zero(temp21a); gsl_matrix_set(temp21a, 0, 0, gsl_vector_get(delta_location,0)); gsl_matrix_set(temp21a, 1, 0, gsl_vector_get(delta_location,1)); gsl_matrix_set(temp21a, 2, 0, gsl_vector_get(delta_location,2)); gsl_matrix_set(temp21a, 3, 0, gsl_vector_get(velocity,0)); gsl_matrix_set(temp21a, 4, 0, gsl_vector_get(velocity,1)); gsl_matrix_set(temp21a, 5, 0, gsl_vector_get(velocity,2)); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, temp21a, pseudo_inverse(state_mean), 0, observation_transformation); //observation_mean[k] = observation_transformation * state_mean[k-1] gsl_matrix_memcpy(temp21a, state_mean); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp21a, 0, observation_mean); //observation_covariance[k] = observation_transformation * state_covariance * transpose(observation_transformation) /* gsl_matrix_set_zero(temp22a); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, observation_transformation, 0, temp22a); //notice observation_transformation is transposed gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, observation_transformation, temp22a, 0, observation_covariance); */ gsl_matrix_set(observation_covariance, 0, 3, timestep*3); gsl_matrix_set(observation_covariance, 3, 0, timestep*3); gsl_matrix_set(observation_covariance, 1, 4, timestep*5); gsl_matrix_set(observation_covariance, 4, 1, timestep*5); gsl_matrix_set(observation_covariance, 2, 5, timestep*6); gsl_matrix_set(observation_covariance, 5, 2, timestep*6); //estimate_mean = predict * state_mean + control * acceleration; gsl_matrix_set_zero(estimate_mean); gsl_matrix_set_zero(temp21a); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, state_mean, 0, estimate_mean); gsl_matrix_view test = gsl_matrix_view_vector(acceleration, unfiltered_states, 1); gsl_matrix * tmp = &test.matrix; gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, control, tmp, 0, temp21a); gsl_matrix_add(estimate_mean, temp21a); //estimate_covariance = predict * state_covariance * transpose(predict) + noise; gsl_matrix_set_zero(estimate_covariance); gsl_matrix_set_zero(temp22a); gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1, state_covariance, predict, 0, temp22a); //notice predict is transposed gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, predict, temp22a, 0, estimate_covariance); gsl_matrix_add_constant(estimate_covariance, 0.1); //adxl345 noise, this is completely wrong //kalman_gain = estimate_covariance * pseudoinverse(estimate_covariance + observation_covariance); gsl_matrix_set_zero(kalman_gain); gsl_matrix_set_zero(temp22a); gsl_matrix_memcpy(temp22a, observation_covariance); gsl_matrix_add(temp22a, estimate_covariance); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, estimate_covariance, pseudo_inverse(temp22a), 0, kalman_gain); //state_mean = estimate_mean + kalman_gain * ( observation_mean - estimate_mean ); gsl_matrix_set_zero(state_mean); gsl_matrix_set_zero(temp21a); gsl_matrix_memcpy(temp21a, observation_mean); gsl_matrix_sub(temp21a, estimate_mean); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, temp21a, 0, state_mean); gsl_matrix_add(state_mean, estimate_mean); //state_covariance = estimate_covariance - kalman_gain * ( estimate_covariance ); gsl_matrix_set_zero(state_covariance); gsl_matrix_set_zero(temp22a); gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1, kalman_gain, estimate_covariance, 0, temp22a); gsl_matrix_add(state_covariance, estimate_covariance); gsl_matrix_sub(state_covariance, temp22a); printf("state_mean:"); gsl_matrix_fprintf(stdout, state_mean, "%f"); //printf("state_covariance:"); //gsl_matrix_fprintf(stdout, state_covariance, "%f"); printf("observation_mean:"); gsl_matrix_fprintf(stdout, observation_mean, "%f"); //printf("observation_covariance:"); //gsl_matrix_fprintf(stdout, observation_covariance, "%f"); printf("estimate_mean:"); gsl_matrix_fprintf(stdout, estimate_mean, "%f"); //printf("estimate_covariance:"); //gsl_matrix_fprintf(stdout, estimate_covariance, "%f"); gsl_matrix_set_zero(temp21a); gsl_matrix_memcpy(temp21a, observation_mean); gsl_matrix_sub(temp21a, state_mean); gsl_matrix_div_elements(temp21a, observation_mean); gsl_matrix_mul_elements(temp21a, temp21a); for (int i = states-1; i >= 0; i--) { error += gsl_matrix_get(temp21a, i, 0); } printf("error: %f\n", error); printf("error/time: %f\n", error/t); printf("\n"); usleep(1000000); } }