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();