modify the covariance to make it more stable

rviz_cfg/laser_localization.rviz

@@ -1,232 +0,0 @@
-Panels:
-  - Class: rviz/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Map1
-      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: /mmap
-      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
-        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
-      Buffer Length: 1
-      Class: rviz/Path
-      Color: 25; 255; 0
-      Enabled: true
-      Line Style: Lines
-      Line Width: 0.03
-      Name: Path
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Topic: /mpath
-      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: 22.9162
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.06
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 1.66903
-        Y: -0.217464
-        Z: -0.674569
-      Name: Current View
-      Near Clip Distance: 0.01
-      Pitch: 1.2198
-      Target Frame: base_stabilized
-      Value: Orbit (rviz)
-      Yaw: 3.15041
-    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

src/att_ekf.cpp

@@ -80,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;
+	//cout << "mag K: " << K << endl;
 	x = x + K*(z - H*x);
 	P = (I- K*H)*P.inverse();
 	//cout << "update mag: " << x.transpose() << endl;
@@ -112,7 +112,7 @@ void Att_ekf::update_imu(Vector3d &acc, Vector3d & gyro, double t)
 
 	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;
+	//cout << "imu K: " << K << endl;
 	x = x + K*(z - H*x);
 	P = (I- K*H)*P.inverse();
 	//cout << "update imu: " << x.transpose() << endl;

src/att_ekf_node.cpp

@@ -56,7 +56,7 @@ void imuCallback(const sensor_msgs::ImuConstPtr & msg)
 
 	geometry_msgs::PoseStamped pose_gt;
 	pose_gt.header.stamp = ros::Time(t_gt);
-	pose_gt.header.frame_id = "/world";
+	pose_gt.header.frame_id = "/base_footprint";
  	pose_gt.pose.orientation.w = q_gt.w();
  	pose_gt.pose.orientation.x = q_gt.x();
  	pose_gt.pose.orientation.y = q_gt.y();
@@ -83,7 +83,7 @@ void publish_pose()
 	MatrixXd m = att_ekf.get_rotation_matrix();
  	Quaterniond q = mat2quaternion(m);
  	pose.header.stamp = ros::Time(att_ekf.get_time());
- 	pose.header.frame_id = "/world";
+ 	pose.header.frame_id = "/base_footprint";
  	pose.pose.orientation.w = q.w();
  	pose.pose.orientation.x = q.x();
  	pose.pose.orientation.y = q.y();