Skip to content

Commit

Permalink
Merge branch 'devel'
Browse files Browse the repository at this point in the history
  • Loading branch information
@RossHartley
RossHartley committed Sep 26, 2018
2 parents f576e1d + e64179f commit a05fe5d
Show file tree
Hide file tree
Showing 12 changed files with 40,508 additions and 118 deletions.
3 changes: 2 additions & 1 deletion include/InEKF.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,8 +54,9 @@ class InEKF {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
InEKF();
InEKF(NoiseParams params);
InEKF(RobotState state);
InEKF(RobotState state, const mapIntVector3d& prior_landmarks);
InEKF(RobotState state, NoiseParams params);

RobotState getState();
NoiseParams getNoiseParams();
Expand Down
9 changes: 4 additions & 5 deletions include/RobotState.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,8 @@ class RobotState {
const Eigen::Matrix3d getRotation();
const Eigen::Vector3d getVelocity();
const Eigen::Vector3d getPosition();
const Eigen::Vector3d getAngularVelocityBias();
const Eigen::Vector3d getLinearAccelerationBias();
const Eigen::Vector3d getGyroscopeBias();
const Eigen::Vector3d getAccelerometerBias();
const int dimX();
const int dimTheta();
const int dimP();
Expand All @@ -53,12 +53,11 @@ class RobotState {
void setRotation(const Eigen::Matrix3d& R);
void setVelocity(const Eigen::Vector3d& v);
void setPosition(const Eigen::Vector3d& p);
void setAngularVelocityBias(const Eigen::Vector3d& bg);
void setLinearAccelerationBias(const Eigen::Vector3d& ba);
void setGyroscopeBias(const Eigen::Vector3d& bg);
void setAccelerometerBias(const Eigen::Vector3d& ba);

void copyDiagX(int n, Eigen::MatrixXd& BigX);

friend class InEKF;
friend std::ostream& operator<<(std::ostream& os, const RobotState& s);

private:
Expand Down
4 changes: 2 additions & 2 deletions matlab_example/gen_data_file.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
load('data/measurements/encoders.mat')

%% Write data to file
fileID = fopen('sim_data.txt','w');
fileID = fopen('imu_landmark_measurements.txt','w');
t = angular_velocity.time;
N = length(t);
for i=10:N
Expand Down Expand Up @@ -34,7 +34,7 @@

%% Write data to file
Cov_e = deg2rad(0.5)^2*eye(14);
fileID = fopen('sim_data.txt','w');
fileID = fopen('imu_kinematic_measurements.txt','w');
t = angular_velocity.time;
N = length(t);
for i=10:N
Expand Down
0 matlab_example/sim_data.txt → ...ab_example/imu_kinematic_measurements.txt
View file
File renamed without changes.
20,192 changes: 20,192 additions & 0 deletions matlab_example/imu_landmark_measurements.txt
View file

Large diffs are not rendered by default.

11 changes: 7 additions & 4 deletions src/InEKF.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,11 +42,14 @@ ostream& operator<<(ostream& os, const Observation& o) {
// Default constructor
InEKF::InEKF() {}

// Constructor with noise params
InEKF::InEKF(NoiseParams params) : noise_params_(params) {}

// Constructor with initial state
InEKF::InEKF(RobotState state) : state_(state) {}

// Constructor wtih initial state and prior landmarks
InEKF::InEKF(RobotState state, const mapIntVector3d& prior_landmarks) : state_(state), prior_landmarks_(prior_landmarks) {}
// Constructor with initial state and noise params
InEKF::InEKF(RobotState state, NoiseParams params) : state_(state), noise_params_(params) {}

// Return robot's current state
RobotState InEKF::getState() {
Expand Down Expand Up @@ -114,8 +117,8 @@ std::map<int,bool> InEKF::getContacts() {
// InEKF Propagation - Inertial Data
void InEKF::Propagate(const Eigen::Matrix<double,6,1>& m, double dt) {

Eigen::Vector3d w = m.head(3) - state_.getAngularVelocityBias(); // Angular Velocity
Eigen::Vector3d a = m.tail(3) - state_.getLinearAccelerationBias(); // Linear Acceleration
Eigen::Vector3d w = m.head(3) - state_.getGyroscopeBias(); // Angular Velocity
Eigen::Vector3d a = m.tail(3) - state_.getAccelerometerBias(); // Linear Acceleration

Eigen::MatrixXd X = state_.getX();
Eigen::MatrixXd P = state_.getP();
Expand Down
8 changes: 4 additions & 4 deletions src/RobotState.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,11 +98,11 @@ const Eigen::Vector3d RobotState::getPosition() {
unique_lock<mutex> mlock(mutex_);
return X_.block<3,1>(0,4);
}
const Eigen::Vector3d RobotState::getAngularVelocityBias() {
const Eigen::Vector3d RobotState::getGyroscopeBias() {
unique_lock<mutex> mlock(mutex_);
return Theta_.head(3);
}
const Eigen::Vector3d RobotState::getLinearAccelerationBias() {
const Eigen::Vector3d RobotState::getAccelerometerBias() {
unique_lock<mutex> mlock(mutex_);
return Theta_.tail(3);
}
Expand Down Expand Up @@ -143,11 +143,11 @@ void RobotState::setPosition(const Eigen::Vector3d& p) {
unique_lock<mutex> mlock(mutex_);
X_.block<3,1>(0,4) = p;
}
void RobotState::setAngularVelocityBias(const Eigen::Vector3d& bg) {
void RobotState::setGyroscopeBias(const Eigen::Vector3d& bg) {
unique_lock<mutex> mlock(mutex_);
Theta_.head(3) = bg;
}
void RobotState::setLinearAccelerationBias(const Eigen::Vector3d& ba) {
void RobotState::setAccelerometerBias(const Eigen::Vector3d& ba) {
unique_lock<mutex> mlock(mutex_);
Theta_.tail(3) = ba;
}
Expand Down
27 changes: 0 additions & 27 deletions src/data/kinematic_measurements.txt → src/data/imu_kinematic_measurements.txt
View file
Original file line number Diff line number Diff line change
@@ -1,30 +1,3 @@
IMU 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000
CONTACT 0.000000 0.000000 1.000000 1.000000 1.000000
KINEMATIC 0.000000 0.000000 0.000000 0.642788 -0.766044 -0.000950 1.000000 0.000000 0.000000 -0.130500 0.000000 -0.766044 -0.642788 1.051240 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000217 -0.000000 -0.000014 0.000000 0.000000 0.000000 -0.000000 0.000098 -0.000000 0.000000 0.000000 0.000000 -0.000014 -0.000000 0.000001 1.000000 0.000000 0.642788 -0.766044 -0.000950 1.000000 0.000000 0.000000 0.130500 0.000000 -0.766044 -0.642788 1.051240 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000217 0.000000 -0.000014 0.000000 0.000000 0.000000 0.000000 0.000098 0.000000 0.000000 0.000000 0.000000 -0.000014 0.000000 0.000001
IMU 0.000500 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000
CONTACT 0.000500 0.000000 1.000000 1.000000 1.000000
KINEMATIC 0.000500 0.000000 0.000000 0.642788 -0.766044 -0.000950 1.000000 0.000000 0.000000 -0.130500 0.000000 -0.766044 -0.642788 1.051240 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000217 -0.000000 -0.000014 0.000000 0.000000 0.000000 -0.000000 0.000098 -0.000000 0.000000 0.000000 0.000000 -0.000014 -0.000000 0.000001 1.000000 0.000000 0.642788 -0.766044 -0.000950 1.000000 0.000000 0.000000 0.130500 0.000000 -0.766044 -0.642788 1.051240 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000217 0.000000 -0.000014 0.000000 0.000000 0.000000 0.000000 0.000098 0.000000 0.000000 0.000000 0.000000 -0.000014 0.000000 0.000001
IMU 0.001000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000
CONTACT 0.001000 0.000000 0.000000 1.000000 0.000000
KINEMATIC 0.001000 0.000000 0.000000 1.000000 -0.000079 -0.031627 0.999990 0.000000 0.004510 -0.135010 0.004510 -0.000079 -0.999990 0.920117 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 0.000000 -0.000010 0.000000 0.000000 0.000000 0.000000 0.000076 -0.000000 0.000000 0.000000 0.000000 -0.000010 -0.000000 0.000005 1.000000 0.000000 1.000000 -0.000079 -0.031627 0.999990 -0.000000 -0.004510 0.135010 -0.004510 -0.000079 -0.999990 0.920117 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 -0.000000 -0.000010 0.000000 0.000000 0.000000 -0.000000 0.000076 0.000000 0.000000 0.000000 0.000000 -0.000010 0.000000 0.000005
IMU 0.001500 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000
CONTACT 0.001500 0.000000 0.000000 1.000000 0.000000
KINEMATIC 0.001500 0.000000 0.000000 1.000000 0.000043 -0.031607 0.999990 -0.000000 0.004510 -0.135010 0.004510 0.000043 -0.999990 0.920117 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 0.000000 -0.000010 0.000000 0.000000 0.000000 0.000000 0.000076 -0.000000 0.000000 0.000000 0.000000 -0.000010 -0.000000 0.000005 1.000000 0.000000 1.000000 0.000043 -0.031607 0.999990 0.000000 -0.004510 0.135010 -0.004510 0.000043 -0.999990 0.920117 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 -0.000000 -0.000010 0.000000 0.000000 0.000000 -0.000000 0.000076 0.000000 0.000000 0.000000 0.000000 -0.000010 0.000000 0.000005
IMU 0.002000 -0.009715 0.019331 -0.000959 7.710939 17.200819 -3.674830
CONTACT 0.002000 0.000000 0.000000 1.000000 0.000000
KINEMATIC 0.002000 0.000000 0.000000 1.000000 0.000700 -0.031646 0.999990 -0.000003 0.004510 -0.135010 0.004510 0.000700 -0.999989 0.920127 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 0.000000 -0.000010 0.000000 0.000000 0.000000 0.000000 0.000076 -0.000000 0.000000 0.000000 0.000000 -0.000010 -0.000000 0.000005 1.000000 0.000000 1.000000 0.000809 -0.031651 0.999990 0.000004 -0.004479 0.134980 -0.004479 0.000809 -0.999990 0.920129 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 -0.000000 -0.000010 0.000000 0.000000 0.000000 -0.000000 0.000076 0.000000 0.000000 0.000000 0.000000 -0.000010 0.000000 0.000005
IMU 0.002500 -0.009715 0.019331 -0.000959 7.710939 17.200819 -3.674830
CONTACT 0.002500 0.000000 0.000000 1.000000 0.000000
KINEMATIC 0.002500 0.000000 0.000000 0.999998 0.001685 -0.031719 0.999990 -0.000008 0.004510 -0.135010 0.004510 0.001685 -0.999988 0.920139 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 0.000000 -0.000010 0.000000 0.000000 0.000000 0.000000 0.000076 -0.000000 0.000000 0.000000 0.000000 -0.000010 -0.000000 0.000005 1.000000 0.000000 0.999996 0.002716 -0.031781 0.999990 0.000012 -0.004449 0.134949 -0.004449 0.002716 -0.999986 0.920156 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 -0.000000 -0.000010 0.000000 0.000000 0.000000 -0.000000 0.000076 0.000000 0.000000 0.000000 0.000000 -0.000010 0.000000 0.000005
IMU 0.003000 -0.009715 0.019331 -0.000959 7.710939 17.200819 -3.674830
CONTACT 0.003000 0.000000 0.000000 1.000000 0.000000
KINEMATIC 0.003000 0.000000 0.000000 0.999998 0.001900 -0.031740 0.999990 -0.000009 0.004510 -0.135010 0.004510 0.001900 -0.999988 0.920143 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 0.000000 -0.000010 0.000000 0.000000 0.000000 0.000000 0.000076 -0.000000 0.000000 0.000000 0.000000 -0.000010 -0.000000 0.000005 1.000000 0.000031 0.999995 0.003077 -0.031769 0.999990 -0.000017 -0.004418 0.134917 -0.004418 0.003077 -0.999985 0.920162 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 -0.000000 -0.000010 0.000000 0.000000 0.000000 -0.000000 0.000076 0.000000 0.000000 0.000000 0.000000 -0.000010 0.000000 0.000005
IMU 0.003500 -0.009955 0.030229 -0.002398 -1.406005 -13.395207 0.502421
CONTACT 0.003500 0.000000 0.000000 1.000000 0.000000
KINEMATIC 0.003500 0.000000 0.000000 0.999996 0.002698 -0.031793 0.999990 -0.000012 0.004479 -0.134980 0.004479 0.002698 -0.999986 0.920155 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 0.000000 -0.000010 0.000000 0.000000 0.000000 0.000000 0.000076 -0.000000 0.000000 0.000000 0.000000 -0.000010 -0.000000 0.000005 1.000000 0.000031 0.999989 0.004566 -0.031874 0.999990 -0.000011 -0.004418 0.134917 -0.004418 0.004566 -0.999980 0.920184 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 -0.000000 -0.000010 0.000000 0.000000 0.000000 -0.000000 0.000076 0.000000 0.000000 0.000000 0.000000 -0.000010 0.000000 0.000005
IMU 0.004000 -0.009955 0.030229 -0.002398 -1.406005 -13.395207 0.502421
CONTACT 0.004000 0.000000 0.000000 1.000000 0.000000
KINEMATIC 0.004000 0.000000 -0.000031 0.999994 0.003444 -0.031838 0.999990 0.000015 0.004418 -0.134917 0.004418 0.003444 -0.999984 0.920167 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 0.000000 -0.000010 0.000000 0.000000 0.000000 0.000000 0.000076 -0.000000 0.000000 0.000000 0.000000 -0.000010 -0.000000 0.000005 1.000000 0.000031 0.999983 0.005822 -0.031914 0.999990 -0.000005 -0.004387 0.134886 -0.004387 0.005822 -0.999973 0.920200 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 -0.000000 -0.000010 0.000000 0.000000 0.000000 -0.000000 0.000076 0.000000 0.000000 0.000000 0.000000 -0.000010 0.000000 0.000005
IMU 0.004500 0.018803 0.040072 -0.004129 -1.887827 0.137898 2.069346
CONTACT 0.004500 0.000000 0.000000 1.000000 0.000000
KINEMATIC 0.004500 0.000000 -0.000061 0.999990 0.004511 -0.031945 0.999991 0.000042 0.004357 -0.134854 0.004356 0.004511 -0.999980 0.920186 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 0.000000 -0.000010 0.000000 0.000000 0.000000 0.000000 0.000076 -0.000000 0.000000 0.000000 0.000000 -0.000010 -0.000000 0.000005 1.000000 0.000061 0.999972 0.007526 -0.032064 0.999991 -0.000029 -0.004357 0.134854 -0.004356 0.007526 -0.999962 0.920225 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000164 -0.000000 -0.000010 0.000000 0.000000 0.000000 -0.000000 0.000076 0.000000 0.000000 0.000000 0.000000 -0.000010 0.000000 0.000005
Expand Down
Loading

0 comments on commit a05fe5d

Please sign in to comment.