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);
}
}
