diff --git a/src/bno055_imu_ros_driver.cpp b/src/bno055_imu_ros_driver.cpp
index 1b03935cb095e0ef26bdd2ac6071998b2982e6f9..90eb4a05c6a3912d9dc7bdee8a60d1311d4af669 100644
--- a/src/bno055_imu_ros_driver.cpp
+++ b/src/bno055_imu_ros_driver.cpp
@@ -57,6 +57,9 @@ bool Bno055ImuDriver::startDriver(void)
       this->imu_device->load_calibration(this->config_.cal_filename);
       this->imu_device->set_operation_mode((op_mode_t)this->config_.mode);
       this->imu_device->set_data_rate(this->config_.update_rate);
+      this->imu_device->set_accel_config(this->config_.accel_range,this->config_.accel_bandwidth,this->config_.accel_pwr_mode);
+      this->imu_device->set_gyro_config(this->config_.gyro_range,this->config_.gyro_bandwidth,this->config_.gyro_pwr_mode);
+      this->imu_device->set_mag_config(this->config_.mag_rate,this->config_.mag_op_mode,this->config_.mag_pwr_mode);
       this->unlock();
       return true;
     }catch(CException &e){
@@ -70,6 +73,9 @@ bool Bno055ImuDriver::startDriver(void)
     try{
       this->imu_device->set_operation_mode(config_mode);
       this->imu_device->set_data_rate(this->config_.update_rate);
+      this->imu_device->set_accel_config(this->config_.accel_range,this->config_.accel_bandwidth,this->config_.accel_pwr_mode);
+      this->imu_device->set_gyro_config(this->config_.gyro_range,this->config_.gyro_bandwidth,this->config_.gyro_pwr_mode);
+      this->imu_device->set_mag_config(this->config_.mag_rate,this->config_.mag_op_mode,this->config_.mag_pwr_mode);
       this->imu_device->set_operation_mode((op_mode_t)this->config_.mode);
       this->unlock();
       return true;
@@ -100,6 +106,7 @@ void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
   
   // depending on current state
   // update driver with new_cfg data
+  this->frame_id=new_cfg.tf_prefix+new_cfg.frame_id;
   switch(this->getState())
   {
     case iri_base_driver::CLOSED:
@@ -107,7 +114,6 @@ void Bno055ImuDriver::config_update(Config& new_cfg, uint32_t level)
 
     case iri_base_driver::OPENED:
       this->imu_device->set_data_rate(new_cfg.update_rate);
-      this->frame_id=new_cfg.tf_prefix+"/"+new_cfg.frame_id;
       // change configuration
       this->imu_device->set_accel_config(new_cfg.accel_range,new_cfg.accel_bandwidth,new_cfg.accel_pwr_mode);
       this->imu_device->set_gyro_config(new_cfg.gyro_range,new_cfg.gyro_bandwidth,new_cfg.gyro_pwr_mode);