From c0727bbe1404e788b4ae82f6c6d99e105ecfacdf Mon Sep 17 00:00:00 2001 From: Martin Miller Date: Sat, 18 Mar 2017 18:09:39 -0500 Subject: Add fake z kalman update --- src/body.cpp | 15 +++++++++++--- src/body.h | 3 ++- src/main.cpp | 64 +++++++++++++++++++++++++++++++++++------------------------ src/main.h | 19 +++++++++--------- src/state.cpp | 13 ++++++++++++ src/state.h | 3 +++ 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 #include "body.h" +Matrix +Body::R() +{ + Matrix R; + R << 1e-8; + return R; +} + Matrix Body::Q (double dt) { @@ -56,11 +64,12 @@ Body::H ( Matrix &H ) * Description: Writes the predicted measurement vector into h. *-------------------------------------------------------------------------------------- */ -void -Body::h ( const Matrix &X, Matrix &h ) +Matrix +Body::h ( const Matrix &X) { + Matrix 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 F(const Matrix &X, const Vector3d &ang, const Quaternion &q); void skewSymmetric(const Vector3d &x, Matrix &y); void H(Matrix &H); - void h(const Matrix &X, Matrix &h); + Matrix h(const Matrix &X); Matrix Q(double dt); + Matrix 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 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 &P, const Eigen::Quaternion &q) +covCallback(const message &msg, Matrix &P, const Quaternion &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 &X, - Eigen::Matrix &P, const Eigen::Quaternion &q, +imuCallback(const message &msg, Matrix &X, + Matrix &P, const Quaternion &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 &X, State mu; double dtf = dt.secs+dt.nsecs*1e-9; - Eigen::Matrix F; + Matrix 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 &X, Eigen::Quaternion &q) +pvaCallback(const message &msg, Matrix &X, Quaternion &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(R); + q = Quaternion(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 &X) +utmCallback(const message &msg, Matrix &X, Matrix &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 z,h; + Matrix 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 qbw; - Eigen::Matrix bodyStateVector; - Eigen::Matrix P; + Quaternion qbw; + Matrix bodyStateVector; + Matrix 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 &P, - const Eigen::Quaternion &q); +void covCallback(const message &msg, Matrix &P, const Quaternion &q); void imgCallback(const message *msg); -void imuCallback(const message &msg, Eigen::Matrix &X, - Eigen::Matrix &P, const Eigen::Quaternion &q, - const timestamp dt); -void pvaCallback(const message &msg, Eigen::Matrix &X, - Eigen::Quaternion &q); -void utmCallback(const message &msg, Eigen::Matrix &X); +void imuCallback(const message &msg, Matrix &X, Matrix &P, + const Quaternion &q, const timestamp dt); +void pvaCallback(const message &msg, Matrix &X, Quaternion &q); +void utmCallback(const message &msg, Matrix &X, Matrix &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 &X, Matrix &P, + const Matrix H, const Matrix &h, + const Matrix &z, const Matrix &R) +{ + Matrix S; + Matrix K; + S = H*P*H.transpose() + R; + K = P*H.transpose()*S.inverse(); + P = (Matrix::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 &P, const Matrix &F, const Matrix &Q ); + void update (Matrix &X, Matrix &P, + const Matrix H, const Matrix &h, + const Matrix &z, const Matrix &R); protected: /* ==================== METHODS ======================================= */ -- cgit v1.1