1 files changed, 75 insertions, 8 deletions

diff --git a/src/state.cpp b/src/state.cpp
index 294b522..304e5ed 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -17,18 +17,46 @@
  */
 #include "state.h"
 
+State::State ( )
+{
+    body = new Body;
+    P = Matrix<double,Dynamic,Dynamic>::Zero(9,9);
+    P.block(6,6,3,3) = 1e-6*Matrix3d::Identity();
+    return ;
+}		/* -----  end of method State::State  ----- */
+
+void
+State::vel ( const Matrix<double,3,1> &v )
+{
+    body->vel(v);
+    return ;
+}		/* -----  end of method State::vel  ----- */
+
+void
+State::enu ( const UTM &utm )
+{
+    utm_c = utm.zone_c;
+    utm_i = utm.zone_i;
+    body->enu(utm);
+    return ;
+}		/* -----  end of method State::enu  ----- */
+
 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)
+State::update ( const Matrix<double,1,1> &z )
 {
+    Matrix<double,1,9> H;
+    H=body->H();
     Matrix<double,1,1> S;
+    S=body->S(P);
     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);
+    Matrix<double,9,1> dx;
+    Matrix<double,1,1> h;
+    h = body->h();
+    dx = K*(z-h);
+    body->update(dx);
 }
 
 /*
@@ -38,13 +66,52 @@ State::update (Matrix<double,9,1> &X, Matrix<double,9,9> &P,
  * Description:  Updates P_k|k-1
  *--------------------------------------------------------------------------------------
  */
-    void
-State::Pkk1 ( Matrix<double,9,9> &P, const Matrix<double,9,9> &F,
-        const Matrix<double,9,9> &Q )
+void
+State::Pkk1 ( const Vector3d &ang, const Quaternion<double> &q, const double dt)
 {
+    Matrix<double,9,9> F,Q;
+    Q = body->Q(dt);
+    F = body->F(ang,q);
     P = F*P*F.transpose()+Q;
     // Enforce symmetry
     P = 0.5*(P+P.transpose());
     return ;
 }		/* -----  end of method State::Pkk1  ----- */
 
+void
+State::motionModel ( const Vector3d &acc, const Vector3d &ang,
+    const Quaternion<double> &q, const double dt)
+{
+    Pkk1(ang,q,dt);
+    body->motionModel(acc,ang,q,dt);
+    return ;
+}		/* -----  end of method State::motionModel  ----- */
+
+void
+State::position_covariance ( const Matrix<double,3,3> &p )
+{
+    P.block(0,0,3,3) = p;
+    return ;
+}		/* -----  end of method State::position_covariance  ----- */
+
+void
+State::velocity_covariance ( const Matrix<double,3,3> &p )
+{
+    P.block(3,3,3,3) = p;
+    return ;
+}		/* -----  end of method State::velocity_covariance  ----- */
+
+void
+State::unicsv ( )
+{
+    body->unicsv();
+    return ;
+}		/* -----  end of method State::unicsv  ----- */
+
+void
+State::accelerometer_bias ( const Vector3d &b)
+{
+    body->accelerometer_bias(b);
+    return ;
+}		/* -----  end of method State::accelerometer_bias  ----- */
+