modify the covariance to make it more stable
This commit is contained in:
libing64
parent
54541e198f
commit
f248e73cc2
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue