34 #include "std_msgs/String.h"
42 #include <boost/format.hpp>
43 #include <kfilter/ekfilter.hpp>
44 #include <kfilter/kvector.hpp>
48 #include <imu_network/sensors_network.h>
49 #include <imu_network/filtered_imu_network.h>
50 #include <imu_network/filtered_imu.h>
53 #define MAX_MEAN_IT 64 //number of iterations to measure the mean values from gyroscopes
55 #define GYRO_CONVERT M_PI/180*0.069565 // conversion of gyro's values from ADC to rad/sec
62 class imuEKF_sp :
public Kalman::EKFilter<double,0,false,false,false>
68 setDim(6, 0, 6, 3, 3);
76 for (
int i = 0 ; i<6 ; i++)
78 for(
int j = 0 ; j<6 ; j++)
121 if(i==j) R(i,j)=75*M_PI/180;
134 if(i==j) W(i,j) = 1.0;
146 if(i==j) Q(i,j) = 1.0;
155 x_(0) = x(0) + x(1)*Period;
157 x_(2) = x(2) + x(3)*Period;
159 x_(4) = x(0) + x(5)*Period;
178 imuEKF_sp::Vector
_z(3);
179 imuEKF_sp::Vector
_u;
208 imu_network::filtered_imu kalman_imu_data;
211 time_stamp.push_back(msg->sensors_val[i].header.stamp);
213 gyro_valy.push_back(msg->sensors_val[i].S_Gy*GYRO_CONVERT);
214 gyro_valz.push_back(msg->sensors_val[i].S_Gz*GYRO_CONVERT);
219 imuEKF_sp::Vector _x(6);
221 imuEKF_sp::Matrix _P0(6,6);
230 for (
int a = 0; a<6;a++)
232 for (
int b=0;b<6;b++)
234 if (a==b) _P0(a,b)=10.3;
244 imuEKF_sp::Vector _x(6);
283 sensor_var[i][0]=msg->sensors_val[i].S_Ax;
284 sensor_var[i][1]=msg->sensors_val[i].S_Ay;
285 sensor_var[i][2]=msg->sensors_val[i].S_Az;
286 sensor_var[i][3]=msg->sensors_val[i].S_Mx;
287 sensor_var[i][4]=msg->sensors_val[i].S_My;
288 sensor_var[i][5]=msg->sensors_val[i].S_Mz;
289 sensor_var[i][6]=msg->sensors_val[i].S_Gx;
290 sensor_var[i][7]=msg->sensors_val[i].S_Gy;
291 sensor_var[i][8]=msg->sensors_val[i].S_Gz;
299 imuEKF_sp::Vector result(6);
323 kalman_data_network.filtered_imu_network[i].header.stamp = msg->sensors_val[i].header.stamp;
335 int main(
int argc,
char **argv)
337 ros::init(argc, argv,
"kalman_imu_raw");
338 n =
new(ros::NodeHandle);
339 ros::Subscriber sub =
n->subscribe(
"topic_raw_data", 1000,
chatterCallback);
340 chatter_pub =
n->advertise<imu_network::filtered_imu_network>(
"topic_filtered_imu", 1000);
ros::Publisher chatter_pub
vector< float > gyro_valy
vector< imuEKF_sp > kalman_imu
vector< float > gyro_real_mean[3]
void chatterCallback(const imu_network::sensors_network::ConstPtr &msg)
vector< float > gyro_mean_vect[MAX_MEAN_IT][3]
vector< ros::Time > time_stamp
int main(int argc, char **argv)
vector< float > gyro_valz
imu_network::filtered_imu_network kalman_data_network
vector< float > gyro_valx