Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

vector2f type assigned with slice type #1036

Open
garlinplus opened this issue Mar 21, 2022 · 0 comments
Open

vector2f type assigned with slice type #1036

garlinplus opened this issue Mar 21, 2022 · 0 comments

Comments

@garlinplus
Copy link

garlinplus commented Mar 21, 2022

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.

	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;
}

private:
size_t _x0, _y0;
Matrix<Type,M,N>* _data;
};
}

Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant