/// \brief Nonlinear process model for needle steering
void UnscentedKalmanFilter::f(vnl_vector<double> x1, vnl_vector<double> u,
vnl_vector<double> &x2)
{
// initialize the output to 0
x2.fill(0.0);
// isolate the position and orientation components of the input vector
vnl_vector<double> p = x1.extract(3,0);
vnl_vector<double> r = x1.extract(3,3);
// change rotation vector representation to quaternion
vnl_vector<double> r_norm = r;
r_norm.normalize();
vnl_quaternion<double> q(r_norm,r.magnitude());
// isolate specific input variables
double d_th = u[0];
double ro = u[1];
double l = u[2];
// define x,z axes as vectors
vnl_matrix<double> I(3,3); I.set_identity();
vnl_vector<double> x_axis = I.get_column(0);
vnl_vector<double> z_axis = I.get_column(2);
// Update position
// define rotation matrix for d_th about z_axis
vnl_matrix<double> Rz_dth = vnl_rotation_matrix( (d_th*z_axis) );
// define circular trajectory in y-z plane
vnl_vector<double> circ(3,0.0);
circ[1] = ro*(1-cos(l/ro));
circ[2] = ro*sin(l/ro);
// define delta position vector in current frame
vnl_vector<double> d_p = Rz_dth*circ;
// Transform delta vector into world frame using quaternion rotation
vnl_vector<double> d_p_world = q.rotate(d_p);
// add rotated vector to original position
vnl_vector<double> p2 = d_p_world + p;
// Update orientation
// form quaternions for x-axis and z-axis rotations
vnl_quaternion<double> q_b(z_axis,d_th);
vnl_quaternion<double> q_a(x_axis,-l/ro);
// multiply original orientation quaternion
vnl_quaternion<double> q2 = q*q_b*q_a;
vnl_vector<double> r2 = q2.axis()*q2.angle();
// Compose final output
for( int i = 0; i < 3; i++)
{
x2[i] = p2[i];
x2[i+3] = r2[i];
}
}
vnl_double_3x3 RotationMatrix(vnl_double_3 axis, const double angle)
{
//make the norm of axis = angle
axis = axis.normalize();
axis = axis*angle;
vnl_double_3x3 R = vnl_rotation_matrix (axis);
return R;
}
