summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/body.cpp69
-rw-r--r--src/body.h6
-rw-r--r--src/main.cpp55
-rw-r--r--src/main.h7
4 files changed, 131 insertions, 6 deletions
diff --git a/src/body.cpp b/src/body.cpp
index daf4e69..330c0a8 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -19,7 +19,64 @@
#include <Eigen/Dense>
#include "body.h"
- void
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: Body
+ * Method: Body :: H
+ * Description: Writes the Jacobian of the measurement model into H.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Body::H ( Matrix<double,1,9> &H )
+{
+ H = Matrix<double,1,9>::Zero();
+ H[0,2] = 1;
+ return ;
+} /* ----- end of method Body::H ----- */
+
+
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: Body
+ * Method: Body :: h
+ * Description: Writes the predicted measurement vector into h.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Body::h ( const Matrix<double,9,1> &X, Matrix<double,1,1> &h )
+{
+ h[0] = X[2];
+ return ;
+} /* ----- end of method Body::h_hat ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: Body
+ * Method: Body :: F
+ * Description: Computes the Jacobian of the motion model of the body.
+ *--------------------------------------------------------------------------------------
+ */
+void
+Body::F ( const Matrix<double,9,1> &X, const Vector3d &ang, const Quaternion<double> &q )
+{
+ Matrix<double,9,9> F = Matrix<double,9,9>::Zero();
+ Matrix<double,3,3> Rbw(q.toRotationMatrix());
+ Matrix<double,3,3> W;
+ skewSymmetric(ang,W);
+ F.block(0,3,3,3) = Rbw;
+ F.block(3,3,3,3) = -W;
+ F.block(3,6,3,3) = -Matrix<double,3,3>::Identity();
+ return ;
+} /* ----- end of method Body::F ----- */
+
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: Body
+ * Method: Body :: motionModel
+ * Description: Propagates the motion model of the body into vector X.
+ *--------------------------------------------------------------------------------------
+ */
+void
Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt)
{
Vector3d bias(X.segment(6,3));
@@ -41,10 +98,18 @@ Body::motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &
return ;
} /* ----- end of method Body::motionModel ----- */
- void
+/*
+ *--------------------------------------------------------------------------------------
+ * Class: Body
+ * Method: Body :: skewSymmetric
+ * Description: Create the skew symmetric matrix y from the vector x.
+ *--------------------------------------------------------------------------------------
+ */
+void
Body::skewSymmetric ( const Vector3d &x, Matrix<double,3,3> &y )
{
y << 0.,-x[2], x[1], x[2],0.,-x[0],-x[1],x[0],0.;
return ;
} /* ----- end of method Body::skewSymmetric ----- */
+
diff --git a/src/body.h b/src/body.h
index d668957..1fc88ff 100644
--- a/src/body.h
+++ b/src/body.h
@@ -23,8 +23,12 @@ class Body
/* ==================== MUTATORS ======================================= */
/* ==================== OPERATORS ======================================= */
- void motionModel ( Matrix<double,9,1> &X, const Vector3d &acc, const Vector3d &ang, const Quaternion<double> &q, const double dt);
+ void motionModel ( Matrix<double,9,1> &X, const Vector3d &acc,
+ const Vector3d &ang, const Quaternion<double> &q, const double dt);
+ void 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);
protected:
/* ==================== METHODS ======================================= */
diff --git a/src/main.cpp b/src/main.cpp
index 86d87f4..a30ca6c 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -22,15 +22,27 @@
#include "main.h"
bool seenutm=false;
bool seenpva=false;
+Eigen::Matrix<double,9,9> P;
#ifdef USE_ROS
#else /* ----- not USE_ROS ----- */
void
+covCallback(const message &msg, Eigen::Matrix<double,9,9> &P, const Eigen::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;
+}
+
+void
imgCallback(const message *msg)
{
return;
}
+
void
imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt)
{
@@ -113,7 +125,39 @@ 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];
data->msg_type = INSCOVS;
+ sscanf(line,"%lf,%lf,%lf,\
+ %lf,%lf,%lf,\
+ %lf,%lf,%lf,\
+ %lf,%lf,%lf,\
+ %lf,%lf,%lf,\
+ %lf,%lf,%lf,\
+ %lf,%lf,%lf,\
+ %lf,%lf,%lf,\
+ %lf,%lf,%lf",
+ pos, pos+1, pos+2,
+ pos+3, pos+4, pos+5,
+ pos+6, pos+7, pos+8,
+ att, att+1, att+2,
+ att+3, att+4, att+5,
+ att+6, att+7, att+8,
+ vel, vel+1, vel+2,
+ vel+3, vel+4, vel+5,
+ vel+6, vel+7, vel+8);
+ data->covariance.position << pos[0], pos[1], pos[2],
+ pos[3], pos[4], pos[5],
+ pos[6], pos[7], pos[8];
+ data->covariance.attitude << att[0], att[1], att[2],
+ att[3], att[4], att[5],
+ att[6], att[7], att[8];
+ data->covariance.velocity << vel[0], vel[1], vel[2],
+ vel[3], vel[4], vel[5],
+ vel[6], vel[7], vel[8];
} else if (!strcmp(msg_type,"inspvas")) {
data->msg_type = INSPVAS;
sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf",
@@ -168,6 +212,7 @@ main(int argc, char **argv)
{
Eigen::Quaternion<double> qbw;
Eigen::Matrix<double,9,1> bodyStateVector;
+ Eigen::Matrix<double,9,9> P;
// bias in FRD coordinates
bodyStateVector.segment(6,3) << 0.975*-0.03713532, 0.9*0.01465135, -0.00709229;
timestamp dt{0,0};
@@ -176,9 +221,9 @@ main(int argc, char **argv)
#ifdef USE_ROS
#else /* ----- not USE_ROS ----- */
- using std::cout;
- using std::endl;
- using std::cerr;
+using std::cout;
+using std::endl;
+using std::cerr;
printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East\n");
// Read sensors from file
char line[MAXLINE];
@@ -209,6 +254,10 @@ while (scanf("%s", line)!=EOF) {
imuCallback(msg, bodyStateVector, qbw, dt);
break;
+ case INSCOVS:
+ covCallback(msg, P, qbw);
+ cout << P << endl;
+
default:
break;
} /* ----- end switch ----- */
diff --git a/src/main.h b/src/main.h
index 2139248..aa97995 100644
--- a/src/main.h
+++ b/src/main.h
@@ -17,6 +17,11 @@
#define MAXLINE 8192
#define MAXFILENAME 1024
+// A struct for storing PVA covariance.
+typedef struct {
+ Eigen::Matrix3d position,velocity,attitude;
+} covariance_t;
+
// A struct for storing x,y,z data.
typedef struct {
double x,y,z;
@@ -69,6 +74,7 @@ typedef struct {
gps position;
UTM utm;
velocity_t velocity;
+ covariance_t covariance;
} message;
int parseLine(char *line, message *msg);
@@ -77,6 +83,7 @@ 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 imgCallback(const message *msg);
void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt);
void pvaCallback(const message &msg, Eigen::Matrix<double,9,1> &X, Eigen::Quaternion<double> &q);