/// \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; }