You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
{{ message }}
This repository has been archived by the owner on May 1, 2024. It is now read-only.
pos_offset_earth.xy() is slice type and _gps_sample_delayed.pos is vector2f type.
if (_control_status.flags.ev_pos) {
Vector3f ev_pos_obs_var;
Vector2f ev_pos_innov_gates;
// correct position and height for offset relative to IMU
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_ev_sample_delayed.pos -= pos_offset_earth;
// Use an incremental position fusion method for EV position data if GPS is also used
if (_params.fusion_mode & MASK_USE_GPS) {
_fuse_hpos_as_odom = true;
} else {
_fuse_hpos_as_odom = false;
}
if (_fuse_hpos_as_odom) {
if (!_hpos_prev_available) {
// no previous observation available to calculate position change
_hpos_prev_available = true;
} else {
// calculate the change in position since the last measurement
Vector3f ev_delta_pos = _ev_sample_delayed.pos - _pos_meas_prev;
// rotate measurement into body frame is required when fusing with GPS
ev_delta_pos = _R_ev_to_ekf * ev_delta_pos;
// use the change in position since the last measurement
_ev_pos_innov(0) = _state.pos(0) - _hpos_pred_prev(0) - ev_delta_pos(0);
_ev_pos_innov(1) = _state.pos(1) - _hpos_pred_prev(1) - ev_delta_pos(1);
// observation 1-STD error, incremental pos observation is expected to have more uncertainty
Matrix3f ev_pos_var = matrix::diag(_ev_sample_delayed.posVar);
ev_pos_var = _R_ev_to_ekf * ev_pos_var * _R_ev_to_ekf.transpose();
ev_pos_obs_var(0) = fmaxf(ev_pos_var(0, 0), sq(0.5f));
ev_pos_obs_var(1) = fmaxf(ev_pos_var(1, 1), sq(0.5f));
}
// record observation and estimate for use next time
_pos_meas_prev = _ev_sample_delayed.pos;
_hpos_pred_prev = _state.pos.xy();
}
_hpos_pred_prev is vector2f and _state.pos.xy() is slice.slice class is defined as followed:
template <typename Type, size_t P, size_t Q, size_t M, size_t N>
class Slice {
public:
Slice(size_t x0, size_t y0, const Matrix<Type, M, N>* data) :
_x0(x0),
_y0(y0),
_data(const_cast<Matrix<Type, M, N>*>(data)) {
static_assert(P <= M, "Slice rows bigger than backing matrix");
static_assert(Q <= N, "Slice cols bigger than backing matrix");
assert(x0 + P <= M);
assert(y0 + Q <= N);
}
const Type &operator()(size_t i, size_t j) const
{
assert(i < P);
assert(j < Q);
return (*_data)(_x0 + i, _y0 + j);
}
Type &operator()(size_t i, size_t j)
{
assert(i < P);
assert(j < Q);
return (*_data)(_x0 + i, _y0 + j);
}
template<size_t MM, size_t NN>
Slice<Type, P, Q, M, N>& operator=(const Slice<Type, P, Q, MM, NN>& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) = other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N>& operator=(const Matrix<Type, P, Q>& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) = other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N>& operator=(const Type& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) = other;
}
}
return self;
}
// allow assigning vectors to a slice that are in the axis
template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
Slice<Type, 1, Q, M, N>& operator=(const Vector<Type, Q>& other)
{
Slice<Type, 1, Q, M, N>& self = *this;
for (size_t j = 0; j < Q; j++) {
self(0, j) = other(j);
}
return self;
}
template<size_t MM, size_t NN>
Slice<Type, P, Q, M, N>& operator+=(const Slice<Type, P, Q, MM, NN>& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) += other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N>& operator+=(const Matrix<Type, P, Q>& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) += other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N>& operator+=(const Type& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) += other;
}
}
return self;
}
template<size_t MM, size_t NN>
Slice<Type, P, Q, M, N>& operator-=(const Slice<Type, P, Q, MM, NN>& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) -= other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N>& operator-=(const Matrix<Type, P, Q>& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) -= other(i, j);
}
}
return self;
}
Slice<Type, P, Q, M, N>& operator-=(const Type& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) -= other;
}
}
return self;
}
Slice<Type, P, Q, M, N>& operator*=(const Type& other)
{
Slice<Type, P, Q, M, N>& self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
self(i, j) *= other;
}
}
return self;
}
Slice<Type, P, Q, M, N>& operator/=(const Type& other)
{
return operator*=(Type(1) / other);
}
Matrix<Type, P, Q> operator*(const Type& other) const
{
const Slice<Type, P, Q, M, N>& self = *this;
Matrix<Type, P, Q> res;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
res(i, j) = self(i, j) * other;
}
}
return res;
}
Matrix<Type, P, Q> operator/(const Type& other) const
{
const Slice<Type, P, Q, M, N>& self = *this;
return self * (Type(1) / other);
}
template<size_t R, size_t S>
const Slice<Type, R, S, M, N> slice(size_t x0, size_t y0) const
{
return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
}
template<size_t R, size_t S>
Slice<Type, R, S, M, N> slice(size_t x0, size_t y0)
{
return Slice<Type, R, S, M, N>(x0 + _x0, y0 + _y0, _data);
}
void copyTo(Type dst[P*Q]) const
{
const Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
dst[i*N+j] = self(i, j);
}
}
}
void copyToColumnMajor(Type dst[P*Q]) const
{
const Slice<Type, P, Q, M, N> &self = *this;
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
dst[i+(j*M)] = self(i, j);
}
}
}
Vector<Type, P<Q?P:Q> diag() const
{
const Slice<Type, P, Q, M, N>& self = *this;
Vector<Type,P<Q?P:Q> res;
for (size_t j = 0; j < (P<Q?P:Q); j++) {
res(j) = self(j,j);
}
return res;
}
Type norm_squared() const
{
const Slice<Type, P, Q, M, N>& self = *this;
Type accum(0);
for (size_t i = 0; i < P; i++) {
for (size_t j = 0; j < Q; j++) {
accum += self(i, j)*self(i, j);
}
}
return accum;
}
Type norm() const
{
return matrix::sqrt(norm_squared());
}
bool longerThan(Type testVal) const
{
return norm_squared() > testVal*testVal;
}
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
_gps_sample_delayed.pos -= pos_offset_earth.xy();
_gps_sample_delayed.hgt += pos_offset_earth(2);
pos_offset_earth.xy() is slice type and _gps_sample_delayed.pos is vector2f type.
_hpos_pred_prev is vector2f and _state.pos.xy() is slice.slice class is defined as followed:
template <typename Type, size_t P, size_t Q, size_t M, size_t N>
class Slice {
public:
Slice(size_t x0, size_t y0, const Matrix<Type, M, N>* data) :
_x0(x0),
_y0(y0),
_data(const_cast<Matrix<Type, M, N>*>(data)) {
static_assert(P <= M, "Slice rows bigger than backing matrix");
static_assert(Q <= N, "Slice cols bigger than backing matrix");
assert(x0 + P <= M);
assert(y0 + Q <= N);
}
private:
size_t _x0, _y0;
Matrix<Type,M,N>* _data;
};
}
The text was updated successfully, but these errors were encountered: