summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/body.cpp15
-rw-r--r--src/body.h3
-rw-r--r--src/main.cpp64
-rw-r--r--src/main.h19
-rw-r--r--src/state.cpp13
-rw-r--r--src/state.h3
6 files changed, 78 insertions, 39 deletions
diff --git a/src/body.cpp b/src/body.cpp
index 4741911..c4d8a82 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -19,6 +19,14 @@
#include <Eigen/Dense>
#include "body.h"
+Matrix<double,1,1>
+Body::R()
+{
+ Matrix<double,1,1> R;
+ R << 1e-8;
+ return R;
+}
+
Matrix<double,9,9>
Body::Q (double dt)
{
@@ -56,11 +64,12 @@ Body::H ( Matrix<double,1,9> &H )
* Description: Writes the predicted measurement vector into h.
*--------------------------------------------------------------------------------------
*/
-void
-Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h )
+Matrix<double,1,1>
+Body::h ( const Matrix<double,9,1> &X)
{
+ Matrix<double,1,1> h;
h[0] = X[2];
- return ;
+ return h;
} /* ----- end of method Body::h_hat ----- */
/*
diff --git a/src/body.h b/src/body.h
index b5b2aae..04196d2 100644
--- a/src/body.h
+++ b/src/body.h
@@ -28,8 +28,9 @@ class Body
Matrix<double,9,9> F(const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q);
void skewSymmetric(const Vector3d &x, Matrix<double,3,3> &y);
void H(Matrix<double, 1,9> &H);
- void h(const Matrix<double,9,1> &X, Matrix<double,1,1> &h);
+ Matrix<double,1,1> h(const Matrix<double,9,1> &X);
Matrix<double,9,9> Q(double dt);
+ Matrix<double,1,1> R();
protected:
/* ==================== METHODS ======================================= */
diff --git a/src/main.cpp b/src/main.cpp
index 838bab8..c3c6fa9 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,7 +22,6 @@
#include "main.h"
bool seenutm=false;
bool seenpva=false;
-Eigen::Matrix<double,9,9> P;
using std::cout;
using std::endl;
using std::cerr;
@@ -31,13 +30,15 @@ using std::cerr;
#else /* ----- not USE_ROS ----- */
void
-covCallback(const message &msg, Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q)
+covCallback(const message &msg, Matrix<double,9,9> &P, const Quaternion<double> &q)
{
- Eigen::Matrix3d Rbw(q.toRotationMatrix());
- Eigen::Matrix3d Renuned;
- Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
- P.block(0,0,3,3) = Renuned*msg.covariance.position*Renuned.transpose();
- P.block(3,3,3,3) = Rbw.transpose()*msg.covariance.velocity*Rbw;
+ if (seenutm==false || seenpva==false) {
+ Matrix3d Rbw(q.toRotationMatrix());
+ Matrix3d Renuned;
+ Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1;
+ P.block(0,0,3,3) = Renuned*msg.covariance.position*Renuned.transpose();
+ P.block(3,3,3,3) = Rbw.transpose()*msg.covariance.velocity*Rbw;
+ }
}
void
@@ -47,12 +48,11 @@ imgCallback(const message *msg)
}
void
-imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X,
- Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q,
+imuCallback(const message &msg, Matrix<double,9,1> &X,
+ Matrix<double,9,9> &P, const Quaternion<double> &q,
const timestamp dt)
{
if (!seenutm || !seenpva || (dt.secs==0 && dt.nsecs==0)) return;
- using Eigen::Vector3d;
Vector3d acc(msg.linear_acceleration.y,msg.linear_acceleration.x,-msg.linear_acceleration.z);
Vector3d ang(msg.angular_velocity.y,msg.angular_velocity.x,-msg.angular_velocity.z);
// WARNING: This is the bias for 1112-1
@@ -62,43 +62,54 @@ imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X,
State mu;
double dtf = dt.secs+dt.nsecs*1e-9;
- Eigen::Matrix<double,9,9> F;
+ Matrix<double,9,9> F;
F = body.F(X,ang,q);
mu.Pkk1(P,F,body.Q(dtf));
body.motionModel(X,acc,ang,q,dtf);
}
void
-pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q)
+pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q)
{
+ // Update quaternion
using Eigen::AngleAxisd;
- using Eigen::Vector3d;
double roll, pitch, yaw;
attitude_t A=msg.attitude;
roll = M_PI*msg.attitude.roll/180.;
pitch = M_PI*msg.attitude.pitch/180.;
yaw = M_PI*(msg.attitude.yaw)/180.;
- Eigen::Matrix3d R;
+ Matrix3d R;
R = AngleAxisd(yaw, Vector3d::UnitZ())
* AngleAxisd(roll, Vector3d::UnitX())
* AngleAxisd(pitch, Vector3d::UnitY());
- q = Eigen::Quaternion<double>(R);
+ q = Quaternion<double>(R);
+
+ // Set body velocity
if (!seenpva) {
Vector3d vw(msg.velocity.north,msg.velocity.east,-msg.velocity.up);
X.segment(3,3) = R.transpose()*vw;
seenpva=true;
- }
+ }
return;
}
void
-utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X)
+utmCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P)
{
if (!seenutm) {
seenutm=true;
X[0] = msg.utm.northing;
X[1] = msg.utm.easting;
X[2] = -msg.utm.up;
+ } else { // Perform Z measurement
+ State mu;
+ Body body;
+ Matrix<double,1,1> z,h;
+ Matrix<double,1,9> H;
+ z(0) = -msg.utm.up;
+ h = body.h(X);
+ body.H(H);
+ mu.update(X,P,H,h,z,body.R());
}
return;
}
@@ -135,8 +146,6 @@ parseLine(char *line, message *data)
rfn = strsep(&line, ",");
sscanf(rfn, "%s", &data->image_names+1);
} else if (!strcmp(msg_type,"inscovs")) {
- using Eigen::Map;
- using Eigen::Matrix3d;
double pos[9];
double att[9];
double vel[9];
@@ -220,18 +229,18 @@ update_dt(const timestamp t, timestamp *t_old)
int
main(int argc, char **argv)
{
- Eigen::Quaternion<double> qbw;
- Eigen::Matrix<double,9,1> bodyStateVector;
- Eigen::Matrix<double,9,9> P;
+ Quaternion<double> qbw;
+ Matrix<double,9,1> bodyStateVector;
+ Matrix<double,9,9> P;
// bias in FRD coordinates
- bodyStateVector.segment(6,3) << 0.975*-0.03713532, 0.9*0.01465135, -0.00709229;
+ bodyStateVector.segment(6,3) << -0.03713532, 1.125*0.01465135, -1*-0.00709229;
timestamp dt{0,0};
timestamp t_old{0,0};
#ifdef USE_ROS
#else /* ----- not USE_ROS ----- */
-printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East\n");
+printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East,Altitude,Vel-X,Vel-Y,Vel-Z,Bias-X,Bias-Y,Bias-Z\n");
// Read sensors from file
char line[MAXLINE];
while (scanf("%s", line)!=EOF) {
@@ -241,7 +250,7 @@ while (scanf("%s", line)!=EOF) {
switch ( msg.msg_type ) {
case BESTUTM:
dt = update_dt(msg.stamp, &t_old);
- utmCallback(msg,bodyStateVector);
+ utmCallback(msg,bodyStateVector,P);
break;
case IMG:
@@ -253,7 +262,10 @@ while (scanf("%s", line)!=EOF) {
pvaCallback(msg, bodyStateVector, qbw);
//std::cout << bodyStateVector.transpose() << std::endl;
if (seenutm)
- printf("%d,%c,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,bodyStateVector[0],bodyStateVector[1]);
+ printf("%d,%c,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", msg.utm.zone_i,msg.utm.zone_c,
+ bodyStateVector[0],bodyStateVector[1],-bodyStateVector[2],
+ bodyStateVector[3],bodyStateVector[4],bodyStateVector[5],
+ bodyStateVector[6],bodyStateVector[7],bodyStateVector[8]);
break;
case RAWIMUS:
diff --git a/src/main.h b/src/main.h
index d1932c8..36802fd 100644
--- a/src/main.h
+++ b/src/main.h
@@ -18,9 +18,13 @@
#define MAXLINE 8192
#define MAXFILENAME 1024
+using Eigen::Matrix;
+using Eigen::Matrix3d;
+using Eigen::Quaternion;
+using Eigen::Vector3d;
// A struct for storing PVA covariance.
typedef struct {
- Eigen::Matrix3d position,velocity,attitude;
+ Matrix3d position,velocity,attitude;
} covariance_t;
// A struct for storing x,y,z data.
@@ -84,15 +88,12 @@ timestamp update_dt(const timestamp t, timestamp *t_old);
#ifdef USE_ROS
void imuCallback();
#else /* ----- not USE_ROS ----- */
-void covCallback(const message &msg, Eigen::Matrix<double,9,9> &P,
- const Eigen::Quaternion<double> &q);
+void covCallback(const message &msg, Matrix<double,9,9> &P, const Quaternion<double> &q);
void imgCallback(const message *msg);
-void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X,
- Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q,
- const timestamp dt);
-void pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X,
- Eigen::Quaternion<double> &q);
-void utmCallback(const message &msg, Eigen::Matrix<double,9,1> &X);
+void imuCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P,
+ const Quaternion<double> &q, const timestamp dt);
+void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaternion<double> &q);
+void utmCallback(const message &msg, Matrix<double,9,1> &X, Matrix<double,9,9> &P);
#endif /* ----- not USE_ROS ----- */
#endif /* ----- #ifndef main_INC ----- */
diff --git a/src/state.cpp b/src/state.cpp
index 8aadb64..294b522 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -17,6 +17,19 @@
*/
#include "state.h"
+void
+State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P,
+ const Matrix<double,1,9> H, const Matrix<double,1,1> &h,
+ const Matrix<double,1,1> &z, const Matrix<double,1,1> &R)
+{
+ Matrix<double,1,1> S;
+ Matrix<double,9,1> K;
+ S = H*P*H.transpose() + R;
+ K = P*H.transpose()*S.inverse();
+ P = (Matrix<double,9,9>::Identity()-K*H)*P;
+ P = 0.5*(P+P.transpose());
+ X += K*(z-h);
+}
/*
*--------------------------------------------------------------------------------------
diff --git a/src/state.h b/src/state.h
index 3a17ea5..743f5d8 100644
--- a/src/state.h
+++ b/src/state.h
@@ -21,6 +21,9 @@ class State
/* ==================== OPERATORS ======================================= */
void Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F,
const Matrix<double,9,9> &Q );
+ void update (Matrix<double,9,1> &X, Matrix<double,9,9> &P,
+ const Matrix<double,1,9> H, const Matrix<double,1,1> &h,
+ const Matrix<double,1,1> &z, const Matrix<double,1,1> &R);
protected:
/* ==================== METHODS ======================================= */