fix bug of R_imu is missed

2016-02-29 19:24:06 +08:00 · 2016-02-29 19:24:06 +08:00 · 54541e198f
parent 6770c16d6c
commit 54541e198f
5 changed files with 273 additions and 22 deletions
--- a/launch/att_ekf.launch
+++ b/launch/att_ekf.launch
@ -5,6 +5,6 @@
 		<remap from="/imu_bias" to="/raw_imu/bias" />
 	</node>
  	<!-- Start rviz visualization with preset config -->
-<!--   	<node pkg="rviz" type="rviz" name="rviz" args="-d $(find att_ekf)/rviz_cfg/att_ekf.rviz"/> -->
+  	<!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find att_ekf)/rviz_cfg/att_estimation.rviz"/> -->

 </launch>
--- a/rviz_cfg/att_estimation.rviz
+++ 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: <Fixed 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
--- 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;
--- 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<double, 12, 1> x;//anguler velocity, angular acceleration velocity, gravity field, magnetic field 
 	Matrix<double, 12, 12> P;//covariance
-	const Matrix<double, 12, 12> Q = MatrixXd::Identity(12, 12)*0.5;
+	const Matrix<double, 12, 12> Q = MatrixXd::Identity(12, 12)*0.01;

 	const Matrix<double, 3, 3> R_acc = MatrixXd::Identity(3, 3)*acc_cov;
 	const Matrix<double, 3, 3> R_gyro = MatrixXd::Identity(3, 3)*gyro_cov;
--- 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();