georgwi's profile - activity

Hey!

I am looking for the error covariance of two sensors (gyro & accelerometer). Since I do have the data sheets I was wondering if you could tell me how to obtain the value. I do know that the covariance might change but is there a way to estimate it from noise specifications or something similar?

In both cases I have the power spectral density of the noise. Is this the value I need? The information I am referring to is on page 12 and page 13 in this http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A.pdf (document).

Thanks for any suggestions :)

you must also copy the value of w from the target state. The Quaternion consists of 4 values. I probably would put all the conversion and publishing inside the if (fresh_data == true) loop. Or you could publish in the callback function and use q_rot directly

Could you be more specific on what does not work? Since you say getRPY does not work for you I take it you get a compile error? Or does the function simply give false values? I see you got some btQuaternions there. First you have to turn this quaternion in a rotation matrix and then use the accessor getRPY on this matrix. Lets assume btQuaternion quat is the quaternion you want to get roll pitch and yaw from:

double roll, pitch, yaw;
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

Now all you have to do is write this information in a message of your choice and publish it. For some reason the btMatrix3x3 does not seem to have the desired member function, so I use tf::Matrix3x3. Make sure you included all the needed stuff:

#include "tf/transform_datatypes.h"
#include "LinearMath/btMatrix3x3.h"

For the Publishing: If you want to publish inside the VRPN_CALLBACK function you have to create your publisher globally. This is not the best method, but it will probably work fine for you:

geometry_msgs::Vector3 rpy;
rpy.x = roll;
rpy.y = pitch;
rpy.z = yaw;
rpy_publisher.publish(rpy);

and globally:

#include "geometry_msgs/Vector3.h"
ros::Publisher rpy_publisher;

and below your node handle:

rpy_publisher = nh.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);

hope I did not forget something :)

edit: Oh you got it but there are incorrect values. Sorry, but I think I can not help you there. Maybe you could check if the error is in some way related to pi/2 or the values are exchanged. Then you could adapt your coordinate system...

edit2: Did you just initialize q in the main() routine? If yes that might be your problem. You have to write some values in q otherwise you will get only random numbers as output :)

Hey, here is a c++ solution to your problem :) just in case you still have use for it! Needed the transformation from quaternion to rpy-angles too so here is what I put together. Used the code fragment from http://answers.ros.org/question/36977/invalid-arguments-convert-from-quaternion-to-euler/ (here) for the actual transformation.

/****************************************************************************

Conversion from a quaternion to roll, pitch and yaw.

Nodes:
subscribed /rotation_quaternion (message of type geometry_msgs::Quaternion)
published /rpy_angles (message oftype geometry_msgs::Vector3.h)

****************************************************************************/

#include "ros/ros.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_datatypes.h"
#include "LinearMath/btMatrix3x3.h"

// Here I use global publisher and subscriber, since I want to access the
// publisher in the function MsgCallback:
ros::Publisher rpy_publisher;
ros::Subscriber quat_subscriber;

// Function for conversion of quaternion to roll pitch and yaw. The angles
// are published here too.
void MsgCallback(const geometry_msgs::Quaternion msg)
{
    // the incoming geometry_msgs::Quaternion is transformed to a tf::Quaterion
    tf::Quaternion quat;
    tf::quaternionMsgToTF(msg, quat);

    // the tf::Quaternion has a method to acess roll pitch and yaw
    double roll, pitch, yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

    // the found angles are written in a geometry_msgs::Vector3
    geometry_msgs::Vector3 rpy;
    rpy.x = roll;
    rpy.y = pitch;
    rpy.z = yaw;

    // this Vector is then published:
    rpy_publisher.publish(rpy);
    ROS_INFO("published rpy angles: roll=%f pitch=%f yaw=%f", rpy.x, rpy.y, rpy.z);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker");
    ros::NodeHandle n;
    rpy_publisher = n.advertise<geometry_msgs::Vector3>("rpy_angles", 1000);
    quat_subscriber = n.subscribe("rotation_quaternion", 1000, MsgCallback);

    // check for incoming quaternions untill ctrl+c is pressed
    ROS_INFO("waiting for quaternion");
    ros::spin();
    return 0;
}