Go to the documentation of this file.
30 *H1 << I_3x3, Z_3x3, Z_3x3;
32 *H2 << Z_3x3,
R.transpose(), Z_3x3;
34 *H3 << Z_3x3, Z_3x3,
R.transpose();
41 *H1 << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
50 *
H << I_3x3, Z_3x3, Z_3x3;
57 *
H << Z_3x3,
R(), Z_3x3;
64 *
H << Z_3x3, Z_3x3,
R();
75 *
H << D_bv_nRb, Z_3x3, I_3x3;
81 Matrix3
R = this->
R();
83 T <<
R, Z_3x3,
t(), Z_3x3,
R,
v(), Vector6::Zero().transpose(), 1.0;
89 os <<
"R: " <<
state.attitude() <<
"\n";
90 os <<
"p: " <<
state.position().transpose() <<
"\n";
91 os <<
"v: " <<
state.velocity().transpose();
97 std::cout << (
s.empty() ?
s :
s +
" ") << *
this << std::endl;
111 Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb;
113 const Rot3 nRc =
nRb.compose(bRc, H1 ? &D_R_nRb : 0);
117 *H1 << D_R_nRb, Z_3x3, Z_3x3,
126 *H2 << D_bRc_xi, Z_3x3, Z_3x3,
136 Matrix3 D_dR_R, D_dt_R, D_dv_R;
145 *H1 << D_xi_R * D_dR_R, Z_3x3, Z_3x3,
146 D_dt_R, -I_3x3, Z_3x3,
147 D_dv_R, Z_3x3, -I_3x3;
150 *H2 << D_xi_R, Z_3x3, Z_3x3,
151 Z_3x3,
dR.matrix(), Z_3x3,
152 Z_3x3, Z_3x3,
dR.matrix();
159 #define D_R_R(H) (H)->block<3,3>(0,0)
160 #define D_R_t(H) (H)->block<3,3>(0,3)
161 #define D_R_v(H) (H)->block<3,3>(0,6)
162 #define D_t_R(H) (H)->block<3,3>(3,0)
163 #define D_t_t(H) (H)->block<3,3>(3,3)
164 #define D_t_v(H) (H)->block<3,3>(3,6)
165 #define D_v_R(H) (H)->block<3,3>(6,0)
166 #define D_v_t(H) (H)->block<3,3>(6,3)
167 #define D_v_v(H) (H)->block<3,3>(6,6)
175 Matrix39 D_xiP_state;
177 double dt22 = 0.5 *
dt *
dt;
181 dP(
xi) <<
dt * b_v + dt22 * b_acceleration;
182 dV(
xi) <<
dt * b_acceleration;
191 F->middleRows<3>(3) +=
dt *
D_t_t(
F) * D_xiP_state;
200 *G1 = D_newState_xi.middleCols<3>(3) * dt22
201 + D_newState_xi.rightCols<3>() *
dt;
208 *G2 = D_newState_xi.leftCols<3>() *
dt;
216 auto [
nRb, n_t, n_v] = (*this);
218 const double dt2 =
dt *
dt;
223 Matrix3 D_dR_R, D_dP_R, D_dV_R, D_body_nav;
225 dP(n_xi) << ((-dt2) * omega_cross_vel);
226 dV(n_xi) << ((-2.0 *
dt) * omega_cross_vel);
229 dP(n_xi) -= (0.5 * dt2) * omega_cross2_t;
230 dV(n_xi) -=
dt * omega_cross2_t;
234 xi <<
nRb.unrotate(
dR(n_xi),
H ? &D_dR_R : 0,
H ? &D_body_nav : 0),
235 nRb.unrotate(
dP(n_xi),
H ? &D_dP_R : 0),
236 nRb.unrotate(
dV(n_xi),
H ? &D_dV_R : 0);
241 const Matrix3 D_cross_state = Omega *
R();
244 D_t_v(
H) << D_body_nav * (-dt2) * D_cross_state;
246 D_v_v(
H) << D_body_nav * (-2.0 *
dt) * D_cross_state;
249 const Matrix3 D_cross2_state = Omega * D_cross_state;
250 D_t_t(
H) -= D_body_nav * (0.5 * dt2) * D_cross2_state;
251 D_v_t(
H) -= D_body_nav *
dt * D_cross2_state;
264 const double dt22 = 0.5 *
dt *
dt;
267 Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
270 +
dt *
nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
275 xi +=
coriolis(
dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
279 Matrix3 Ri =
nRb.matrix();
284 D_t_R(H1) +=
dt * D_dP_Ri1 + dt22 * D_dP_Ri2;
285 D_t_v(H1) +=
dt * D_dP_nv * Ri;
Class between(const Class &g) const
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange[*:*] noreverse nowriteback set trange[*:*] noreverse nowriteback set urange[*:*] noreverse nowriteback set vrange[*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
static Eigen::Block< Vector9, 3, 1 > dR(Vector9 &v)
NavState update(const Vector3 &b_acceleration, const Vector3 &b_omega, const double dt, OptionalJacobian< 9, 9 > F, OptionalJacobian< 9, 3 > G1, OptionalJacobian< 9, 3 > G2) const
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
std::ostream & operator<<(std::ostream &os, const Dih6 &m)
static NavState FromPoseVelocity(const Pose3 &pose, const Vector3 &vel, OptionalJacobian< 9, 6 > H1, OptionalJacobian< 9, 3 > H2)
Named constructor with derivatives.
Point3 t_
position n_t, in nav frame
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
NavState()
Default constructor.
static Eigen::Block< Vector9, 3, 1 > dP(Vector9 &v)
static const Velocity3 vel(0.4, 0.5, 0.6)
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Eigen::Triplet< double > T
Velocity3 bodyVelocity(OptionalJacobian< 3, 9 > H={}) const
const Vector3 & v() const
Return velocity as Vector3. Computation-free.
ofstream os("timeSchurFactors.csv")
Matrix3 skewSymmetric(double wx, double wy, double wz)
Point3 unrotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
rotate point from world to rotated frame
Vector9 correctPIM(const Vector9 &pim, double dt, const Vector3 &n_gravity, const std::optional< Vector3 > &omegaCoriolis, bool use2ndOrderCoriolis=false, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
const Rot3 & attitude(OptionalJacobian< 3, 9 > H={}) const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
Navigation state composing of attitude, position, and velocity.
Rot3 R_
Rotation nRb, rotates points/velocities in body to points/velocities in nav.
Velocity3 v_
velocity n_v in nav frame
Vector3 t() const
Return position as Vector3.
Matrix3 R() const
Return rotation matrix. Induces computation in quaternion mode.
void g(const string &key, int i)
static Eigen::Block< Vector9, 3, 1 > dV(Vector9 &v)
const Rot3 & rotation(OptionalJacobian< 3, 6 > Hself={}) const
get rotation
bool equals(const NavState &other, double tol=1e-8) const
equals
const Point3 & position(OptionalJacobian< 3, 9 > H={}) const
static Rot3 Expmap(const Vector3 &v, OptionalJacobian< 3, 3 > H={})
Array< int, Dynamic, 1 > v
Matrix3 transpose() const
const Velocity3 & velocity(OptionalJacobian< 3, 9 > H={}) const
NavState retract(const Vector9 &v, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
retract with optional derivatives
static Vector3 Logmap(const Rot3 &R, OptionalJacobian< 3, 3 > H={})
void print(const std::string &s="") const
print
static NavState Create(const Rot3 &R, const Point3 &t, const Velocity3 &v, OptionalJacobian< 9, 3 > H1, OptionalJacobian< 9, 3 > H2, OptionalJacobian< 9, 3 > H3)
Named constructor with derivatives.
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1={}, OptionalJacobian< 9, 9 > H2={}) const
localCoordinates with optional derivatives
Vector9 coriolis(double dt, const Vector3 &omega, bool secondOrder=false, OptionalJacobian< 9, 9 > H={}) const
Compute tangent space contribution due to Coriolis forces.
bool equals(const Rot3 &p, double tol=1e-9) const
Rot2 R(Rot2::fromAngle(0.1))
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:03:11