Go to the documentation of this file.
25 double z,
double vx,
double vy,
double vz) :
53 cout <<
s <<
":" << endl;
55 cout <<
" T" <<
t().transpose() << endl;
61 double max_accel,
double dt)
const {
69 const double yaw2 =
r2.ypr()(0);
72 const double mag_v1 =
v1.norm();
75 double dv =
bound(vel_rate - mag_v1, - (max_accel *
dt), max_accel *
dt);
76 double mag_v2 = mag_v1 + dv;
86 double pitch_rate,
double heading_rate,
double lift_control,
double dt)
const {
92 Vector rot_rates = (
Vector(3) << 0.0, pitch_rate, heading_rate).finished();
96 const double thrust = 50.0;
97 const double lift = 50.0;
98 const double drag = 0.1;
99 double yaw2 =
r2.yaw();
100 double pitch2 =
r2.pitch();
101 double forward_accel = -thrust *
sin(pitch2);
107 - drag * (
Vector(3) <<
v1.x(),
v1.y(), 0.0).finished()
108 + Vector::Unit(3,2)*(loss_lift - lift_control);
142 imu.head<3>() =
r2.transpose() * (accel -
kGravity);
154 imu.tail<3>() = Enb * dR;
171 Matrix36 D_t1_pose, D_t2_other;
173 const Point3 t2 =
other.pose().translation(H2 ? &D_t2_other : 0);
174 Matrix13 D_d_t1, D_d_t2;
175 double d =
distance3(t1, t2, H1 ? &D_d_t1 : 0, H2 ? &D_d_t2 : 0);
176 if (H1) *H1 << D_d_t1 * D_t1_pose, 0,0,0;
177 if (H2) *H2 << D_d_t2 * D_t2_other, 0,0,0;
186 Matrix6 D_newpose_trans, D_newpose_pose;
187 Pose3 newpose =
trans.compose(
pose(), D_newpose_trans, D_newpose_pose);
190 Matrix3 D_newvel_R, D_newvel_v;
195 Dglobal->topLeftCorner<6,6>() = D_newpose_pose;
196 Dglobal->bottomRightCorner<3,3>() = D_newvel_v;
201 Dtrans->topLeftCorner<6,6>() = D_newpose_trans;
202 Dtrans->bottomLeftCorner<3,3>() = D_newvel_R;
204 return PoseRTV(newpose, newvel);
209 assert(euler.size() == 3);
210 const double s1 =
sin(euler.x()),
c1 =
cos(euler.x());
211 const double t2 =
tan(euler.y()),
c2 =
cos(euler.y());
213 Ebn << 1.0, s1 * t2,
c1 * t2,
227 const double s1 =
sin(euler.x()),
c1 =
cos(euler.x());
228 const double s2 =
sin(euler.y()),
c2 =
cos(euler.y());
229 Enb << 1.0, 0.0, -s2,
Vector3 xyz(OptionalJacobian< 3, 3 > H={}) const
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type tail(NType n)
bool equals(const PoseRTV &other, double tol=1e-6) const
PoseRTV transformed_from(const Pose3 &trans, ChartJacobian Dglobal={}, OptionalJacobian< 9, 6 > Dtrans={}) const
typedef and functions to augment Eigen's VectorXd
static const double d[K][N]
Jet< T, N > sin(const Jet< T, N > &f)
Vector3 rpy(OptionalJacobian< 3, 3 > H={}) const
static Matrix RRTMbn(const Vector3 &euler)
double bound(double a, double min, double max)
Vector3 Velocity3
Velocity is currently typedef'd to Vector3.
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 x
static Rot3 Yaw(double t)
Positive yaw is to right (as in aircraft heading). See ypr.
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
PoseRTV generalDynamics(const Vector &accel, const Vector &gyro, double dt) const
General Dynamics update - supply control inputs in body frame.
const Rot3 & rotation() const
Jet< T, N > cos(const Jet< T, N > &f)
Point3 translationIntegration(const Rot3 &r2, const Velocity3 &v2, double dt) const
static Rot2 fromAngle(double theta)
Named constructor from angle in radians.
Vector6 imuPrediction(const PoseRTV &x2, double dt) const
static const Vector kGravity
void print(const Matrix &A, const string &s, ostream &stream)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type segment(Index start, NType n)
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
Point3 rotate(const Point3 &p, OptionalJacobian< 3, 3 > H1={}, OptionalJacobian< 3, 3 > H2={}) const
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
void print(const std::string &s="") const
const Velocity3 & v() const
bool equals(const Pose3 &pose, double tol=1e-9) const
assert equality up to a tolerance
const Point3 & translation(OptionalJacobian< 3, 6 > Hself={}) const
get translation
Pose3 with translational velocity.
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE FixedSegmentReturnType< internal::get_fixed_value< NType >::value >::Type head(NType n)
const EIGEN_DEVICE_FUNC TanReturnType tan() const
const Point3 & translation() const
PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const
const Velocity3 & velocity() const
Matrix trans(const Matrix &A)
static Matrix RRTMnb(const Vector3 &euler)
Class retract(const TangentVector &v) const
retract as required by manifold concept: applies v at *this
const Pose3 & pose() const
double distance3(const Point3 &p1, const Point3 &q, OptionalJacobian< 1, 3 > H1, OptionalJacobian< 1, 3 > H2)
distance between two points
PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const
double range(const PoseRTV &other, OptionalJacobian< 1, 9 > H1={}, OptionalJacobian< 1, 9 > H2={}) const
Pose3 x2(Rot3::Ypr(0.0, 0.0, 0.0), l2)
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:24