diff --git a/launch/att_ekf.launch b/launch/att_ekf.launch index 1f744c8..8eb10ab 100644 --- a/launch/att_ekf.launch +++ b/launch/att_ekf.launch @@ -5,6 +5,6 @@ - + \ No newline at end of file diff --git a/rviz_cfg/att_estimation.rviz b/rviz_cfg/att_estimation.rviz new file mode 100644 index 0000000..5109014 --- /dev/null +++ b/rviz_cfg/att_estimation.rviz @@ -0,0 +1,248 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Pose1 + - /Pose2 + Splitter Ratio: 0.5 + Tree Height: 478 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + front_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + laser0_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: /map + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2 + Min Value: 0 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /front_cam/camera/image + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Value: true + Visibility: + Grid: false + LaserScan: false + Map: false + Path: true + Pose: true + RobotModel: false + Value: true + Zoom Factor: 1 + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 0; 0 + Enabled: true + Line Style: Lines + Line Width: 0.03 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Topic: /trajectory + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /pose + Value: true + - Alpha: 1 + Axes Length: 0.5 + Axes Radius: 0.1 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.3 + Head Radius: 0.1 + Name: Pose + Shaft Length: 1 + Shaft Radius: 0.05 + Shape: Axes + Topic: /pose_gt + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 12.4519 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.65759 + Y: -1.70336 + Z: -0.589262 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.530399 + Target Frame: base_stabilized + Value: Orbit (rviz) + Yaw: 3.21541 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 1026 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000012e00000378fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000026d000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000029b000001050000001600fffffffb0000000a0049006d00610067006501000002d9000000c70000000000000000000000010000010f0000035ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000410000035f000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000065f0000003efc0100000002fb0000000800540069006d006501000000000000065f000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000052b0000037800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1631 + X: 55 + Y: 14 diff --git a/src/att_ekf.cpp b/src/att_ekf.cpp index 61e8019..2e9a422 100644 --- a/src/att_ekf.cpp +++ b/src/att_ekf.cpp @@ -25,7 +25,7 @@ Att_ekf::Att_ekf() P.setZero(); P.block<3, 3>(0, 0) = R_gyro; - P.block<3, 3>(3, 3) = R_gyro*1000; + P.block<3, 3>(3, 3) = R_gyro; P.block<3, 3>(6, 6) = R_acc; P.block<3, 3>(9, 9) = R_mag; } @@ -53,11 +53,9 @@ void Att_ekf::predict(double t) A.block<3, 3>(6, 0) += skew_symmetric(ra)*dt; A.block<3, 3>(9, 0) += skew_symmetric(rm)*dt; - - P = A*P*A.transpose() + Q;//Q? curr_t = t; - //cout << "predict: " << x.transpose() << endl; + } void Att_ekf::update_magnetic(Vector3d& mag, double t) { @@ -82,7 +80,7 @@ void Att_ekf::update_magnetic(Vector3d& mag, double t) MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose() + R_mag).inverse(); MatrixXd I = MatrixXd::Identity(12, 12); - + cout << "mag K: " << K << endl; x = x + K*(z - H*x); P = (I- K*H)*P.inverse(); //cout << "update mag: " << x.transpose() << endl; @@ -112,8 +110,9 @@ void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t) H.block<3, 3>(0, 0) = Matrix3d::Identity(); H.block<3, 3>(3, 6) = Matrix3d::Identity(); - MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose()).inverse(); + MatrixXd K = P.inverse()*H.transpose()*(H*P.inverse()*H.transpose() + R_imu).inverse(); MatrixXd I = MatrixXd::Identity(12, 12); + cout << "imu K: " << K << endl; x = x + K*(z - H*x); P = (I- K*H)*P.inverse(); //cout << "update imu: " << x.transpose() << endl; diff --git a/src/att_ekf.h b/src/att_ekf.h index 7e20cd1..8c9191e 100644 --- a/src/att_ekf.h +++ b/src/att_ekf.h @@ -15,14 +15,14 @@ public: Matrix3d get_rotation_matrix(); double get_time() {return curr_t;} private: - const double acc_cov = 0.35; + const double acc_cov = 10; const double gyro_cov = 0.05; - const double mag_cov = 0.2; + const double mag_cov = 10; double curr_t; Matrix x;//anguler velocity, angular acceleration velocity, gravity field, magnetic field Matrix P;//covariance - const Matrix Q = MatrixXd::Identity(12, 12)*0.5; + const Matrix Q = MatrixXd::Identity(12, 12)*0.01; const Matrix R_acc = MatrixXd::Identity(3, 3)*acc_cov; const Matrix R_gyro = MatrixXd::Identity(3, 3)*gyro_cov; diff --git a/src/att_ekf_node.cpp b/src/att_ekf_node.cpp index 0bc4283..dcc7c5b 100644 --- a/src/att_ekf_node.cpp +++ b/src/att_ekf_node.cpp @@ -25,6 +25,7 @@ Vector3d gyro_bias; Vector3d acc_bias; Quaterniond q_gt; +double t_gt; ros::Publisher pose_pub, pose_gt_pub; void magCallback(const geometry_msgs::Vector3StampedConstPtr& msg) @@ -51,6 +52,16 @@ void imuCallback(const sensor_msgs::ImuConstPtr & msg) q_gt.x() = msg->orientation.x; q_gt.y() = msg->orientation.y; q_gt.z() = msg->orientation.z; + t_gt = msg->header.stamp.toSec(); + + geometry_msgs::PoseStamped pose_gt; + pose_gt.header.stamp = ros::Time(t_gt); + pose_gt.header.frame_id = "/world"; + pose_gt.pose.orientation.w = q_gt.w(); + pose_gt.pose.orientation.x = q_gt.x(); + pose_gt.pose.orientation.y = q_gt.y(); + pose_gt.pose.orientation.z = q_gt.z(); + pose_gt_pub.publish(pose_gt); //cout << "q_gt: " << q_gt.w() << " " << q_gt.vec().transpose() << endl; } @@ -71,22 +82,13 @@ void publish_pose() geometry_msgs::PoseStamped pose; MatrixXd m = att_ekf.get_rotation_matrix(); Quaterniond q = mat2quaternion(m); - pose.header.stamp = ros::Time::now(); - pose.header.frame_id = "/base_link"; + pose.header.stamp = ros::Time(att_ekf.get_time()); + pose.header.frame_id = "/world"; pose.pose.orientation.w = q.w(); pose.pose.orientation.x = q.x(); pose.pose.orientation.y = q.y(); pose.pose.orientation.z = q.z(); pose_pub.publish(pose); - - geometry_msgs::PoseStamped pose_gt; - pose_gt.header.stamp = ros::Time::now(); - pose_gt.header.frame_id = "/base_link"; - pose_gt.pose.orientation.w = q_gt.w(); - pose_gt.pose.orientation.x = q_gt.x(); - pose_gt.pose.orientation.y = q_gt.y(); - pose_gt.pose.orientation.z = q_gt.z(); - pose_gt_pub.publish(pose_gt); } int main(int argc, char **argv) @@ -123,6 +125,7 @@ int main(int argc, char **argv) gyro -= gyro_bias; } att_ekf.update_imu(acc, gyro, t); + publish_pose(); imu_q.pop_front(); }else { @@ -132,6 +135,7 @@ int main(int argc, char **argv) mag(2) = mag_q.front().second.vector.z; double t = mag_q.front().first; att_ekf.update_magnetic(mag, t); + publish_pose(); mag_q.pop_front(); } if(imu_q.size() == 0 || mag_q.size() == 0) break; @@ -141,7 +145,7 @@ int main(int argc, char **argv) // cout << "q: " << q.w() << " " << q.vec().transpose() << endl; // cout << "q_gt: " << q_gt.w() << " " << q_gt.vec().transpose() << endl; - publish_pose(); + //Quaterniond dq = q.inverse()*q_gt; //cout << "dq: " << dq.w() << " " << dq.vec().transpose() << endl; loop_rate.sleep();