{
TYPE euler_data[EULER_SIZE] = {roll, pitch, yaw};
- vector<TYPE> v(EULER_SIZE, euler_data);
+ vect<TYPE> v(EULER_SIZE, euler_data);
m_ang = v;
}
template <typename TYPE>
-euler_angles<TYPE>::euler_angles(const vector<TYPE> v)
+euler_angles<TYPE>::euler_angles(const vect<TYPE> v)
{
m_ang = v;
}
template <typename TYPE>
class euler_angles {
public:
- vector<TYPE> m_ang;
+ vect<TYPE> m_ang;
euler_angles();
euler_angles(const TYPE roll, const TYPE pitch, const TYPE yaw);
- euler_angles(const vector<TYPE> v);
+ euler_angles(const vect<TYPE> v);
euler_angles(const euler_angles<TYPE>& e);
~euler_angles();
std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL);
- vector<TYPE> vec(MOVING_AVERAGE_WINDOW_LENGTH, arr);
- vector<TYPE> vec1x3(V1x3S);
- vector<TYPE> vec1x6(V1x6S);
+ vect<TYPE> vec(MOVING_AVERAGE_WINDOW_LENGTH, arr);
+ vect<TYPE> vec1x3(V1x3S);
+ vect<TYPE> vec1x6(V1x6S);
matrix<TYPE> mat6x6(M6X6R, M6X6C);
m_var_gyr_x = vec;
TYPE a_bias[] = {BIAS_AX, BIAS_AY, BIAS_AZ};
TYPE g_bias[] = {BIAS_GX, BIAS_GY, BIAS_GZ};
- vector<TYPE> acc_data(V1x3S);
- vector<TYPE> gyr_data(V1x3S);
- vector<TYPE> acc_bias(V1x3S, a_bias);
- vector<TYPE> gyr_bias(V1x3S, g_bias);
+ vect<TYPE> acc_data(V1x3S);
+ vect<TYPE> gyr_data(V1x3S);
+ vect<TYPE> acc_bias(V1x3S, a_bias);
+ vect<TYPE> gyr_bias(V1x3S, g_bias);
acc_data = accel.m_data - acc_bias;
gyr_data = (gyro.m_data - gyr_bias) / (TYPE) SCALE_GYRO;
TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
TYPE arr_mag_e[V1x3S] = {0.0, -1.0, 0.0};
- vector<TYPE> acc_e(V1x3S, arr_acc_e);
- vector<TYPE> mag_e(V1x3S, arr_mag_e);
+ vect<TYPE> acc_e(V1x3S, arr_acc_e);
+ vect<TYPE> mag_e(V1x3S, arr_mag_e);
- vector<TYPE> acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data);
- vector<TYPE> acc_e_x_mag_e = cross(acc_e, mag_e);
+ vect<TYPE> acc_b_x_mag_b = cross(m_filt_accel[1].m_data, m_filt_magnetic[1].m_data);
+ vect<TYPE> acc_e_x_mag_e = cross(acc_e, mag_e);
- vector<TYPE> cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data);
- vector<TYPE> cross2 = cross(acc_e_x_mag_e, acc_e);
+ vect<TYPE> cross1 = cross(acc_b_x_mag_b, m_filt_accel[1].m_data);
+ vect<TYPE> cross2 = cross(acc_e_x_mag_e, acc_e);
matrix<TYPE> mat_b(M3X3R, M3X3C);
matrix<TYPE> mat_e(M3X3R, M3X3C);
m_state_old = m_state_new;
TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]};
- vector<TYPE> vec(V1x3S, arr_bias);
+ vect<TYPE> vec(V1x3S, arr_bias);
m_bias_correction = vec;
}
sensor_data<TYPE> m_filt_accel[2];
sensor_data<TYPE> m_filt_gyro[2];
sensor_data<TYPE> m_filt_magnetic[2];
- vector<TYPE> m_var_gyr_x;
- vector<TYPE> m_var_gyr_y;
- vector<TYPE> m_var_gyr_z;
- vector<TYPE> m_var_roll;
- vector<TYPE> m_var_pitch;
- vector<TYPE> m_var_yaw;
+ vect<TYPE> m_var_gyr_x;
+ vect<TYPE> m_var_gyr_y;
+ vect<TYPE> m_var_gyr_z;
+ vect<TYPE> m_var_roll;
+ vect<TYPE> m_var_pitch;
+ vect<TYPE> m_var_yaw;
matrix<TYPE> m_driv_cov;
matrix<TYPE> m_aid_cov;
matrix<TYPE> m_tran_mat;
matrix<TYPE> m_measure_mat;
matrix<TYPE> m_pred_cov;
- vector<TYPE> m_state_new;
- vector<TYPE> m_state_old;
- vector<TYPE> m_state_error;
- vector<TYPE> m_bias_correction;
+ vect<TYPE> m_state_new;
+ vect<TYPE> m_state_old;
+ vect<TYPE> m_state_error;
+ vect<TYPE> m_bias_correction;
quaternion<TYPE> m_quat_aid;
quaternion<TYPE> m_quat_driv;
rotation_matrix<TYPE> m_rot_matrix;
{
TYPE vec_data[QUAT_SIZE] = {w, x, y, z};
- vector<TYPE> v(QUAT_SIZE, vec_data);
+ vect<TYPE> v(QUAT_SIZE, vec_data);
m_quat = v;
}
template <typename TYPE>
-quaternion<TYPE>::quaternion(const vector<TYPE> v)
+quaternion<TYPE>::quaternion(const vect<TYPE> v)
{
m_quat = v;
}
template <typename TYPE>
class quaternion {
public:
- vector<TYPE> m_quat;
+ vect<TYPE> m_quat;
quaternion();
quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z);
- quaternion(const vector<TYPE> v);
+ quaternion(const vect<TYPE> v);
quaternion(const quaternion<TYPE>& q);
~quaternion();
{
TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z};
- vector<TYPE> v(SENSOR_DATA_SIZE, vec_data);
+ vect<TYPE> v(SENSOR_DATA_SIZE, vec_data);
m_data = v;
m_time_stamp = time_stamp;
}
template <typename TYPE>
-sensor_data<TYPE>::sensor_data(const vector<TYPE> v,
+sensor_data<TYPE>::sensor_data(const vect<TYPE> v,
const unsigned long long time_stamp = 0)
{
m_data = v;
template <typename TYPE>
class sensor_data {
public:
- vector<TYPE> m_data;
+ vect<TYPE> m_data;
unsigned long long m_time_stamp;
sensor_data();
sensor_data(const TYPE x, const TYPE y, const TYPE z,
const unsigned long long time_stamp);
- sensor_data(const vector<TYPE> v,
+ sensor_data(const vect<TYPE> v,
const unsigned long long time_stamp);
sensor_data(const sensor_data<TYPE>& s);
~sensor_data();
float arr2[4] = {0.6, 0.6, -.18, -.44};
float arr3[4] = {-0.5, -0.36, .43, .03};
- vector<float> v0(3, arr0);
- vector<float> v1(3, arr1);
- vector<float> v2(4, arr2);
- vector<float> v3(4, arr3);
+ vect<float> v0(3, arr0);
+ vect<float> v1(3, arr1);
+ vect<float> v2(4, arr2);
+ vect<float> v3(4, arr3);
quaternion<float> q1(v2);
quaternion<float> q2(v3);
float arr0[4] = {2344.98, 345.24, 456.12, 98.33};
float arr1[4] = {0.056, 0.34, -0.0076, 0.001};
- vector<float> v0(4, arr0);
- vector<float> v1(4, arr1);
+ vect<float> v0(4, arr0);
+ vect<float> v1(4, arr1);
quaternion<float> q0(v0);
quaternion<float> q1(v1);
{
float arr1[3] = {1.04, -4.678, -2.34};
- vector<float> v1(3, arr1);
+ vect<float> v1(3, arr1);
sensor_data<float> sd1(2.0, 3.0, 4.0, 140737488355328);
sensor_data<float> sd2(1.04, -4.678, -2.34);
float arr43[6] = {2.3454,-0.0384,-8.90,3.455,6.785,21.345};
float arr5[5] = {0.2,-0.4,0.6,-0.8,1.0};
- vector<float> v1(5, arr0);
- vector<float> v2(5, arr8);
- vector<float> v10(4, arr3);
- vector<float> v12(4, arr4);
- vector<float> v15(6, arr1);
- vector<float> v20(6, arr43);
- vector<float> v21(3, arr2);
- vector<float> v22(3, arr15);
- vector<float> v3(4);
- vector<float> v6(3);
- vector<float> v13(5);
- vector<float> v95(6);
- vector<float> v35(5, arr5);
+ vect<float> v1(5, arr0);
+ vect<float> v2(5, arr8);
+ vect<float> v10(4, arr3);
+ vect<float> v12(4, arr4);
+ vect<float> v15(6, arr1);
+ vect<float> v20(6, arr43);
+ vect<float> v21(3, arr2);
+ vect<float> v22(3, arr15);
+ vect<float> v3(4);
+ vect<float> v6(3);
+ vect<float> v13(5);
+ vect<float> v95(6);
+ vect<float> v35(5, arr5);
float arr57[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}};
matrix<float> m12(3, 3, (float *) arr57);
cout<< "\nProduct:\n"<< m102 << endl;
cout<< "\n\n\nVector x Multiplication matrix:\n";
- vector<float> v102 = (v22 * m12);
+ vect<float> v102 = (v22 * m12);
cout<< "\n" << v22 << "\n" << m12;
cout<< "\nProduct:\n" << v102 << endl;
float val = mul(v22, m32);
cout<< "\nOutput:\n" << v3 << endl;
- vector<float> v111 = cross(v21, v22);
+ vect<float> v111 = cross(v21, v22);
cout<< "\n\n\nCross Product:";
cout << "\n\n" << v21 << "\n\n" << v22;
cout << "\nResult:\n\n" << v111;
#if defined (_VECTOR_H) && defined (_MATRIX_H)
template <typename TYPE>
-vector<TYPE>::vector(void)
+vect<TYPE>::vect(void)
{
m_vec = NULL;
}
template <typename TYPE>
-vector<TYPE>::vector(const int size)
+vect<TYPE>::vect(const int size)
{
m_size = size;
m_vec = NULL;
}
template <typename TYPE>
-vector<TYPE>::vector(const int size, TYPE *vec_data)
+vect<TYPE>::vect(const int size, TYPE *vec_data)
{
m_size = size;
m_vec = NULL;
}
template <typename TYPE>
-vector<TYPE>::vector(const vector<TYPE>& v)
+vect<TYPE>::vect(const vect<TYPE>& v)
{
m_size = v.m_size;
m_vec = NULL;
}
template <typename TYPE>
-vector<TYPE>::~vector()
+vect<TYPE>::~vect()
{
if (m_vec != NULL)
delete[] m_vec;
}
template <typename TYPE>
-vector<TYPE> vector<TYPE>::operator =(const vector<TYPE>& v)
+vect<TYPE> vect<TYPE>::operator =(const vect<TYPE>& v)
{
if (this == &v)
{
}
template <typename TYPE>
-ostream& operator <<(ostream& dout, vector<TYPE>& v)
+ostream& operator <<(ostream& dout, vect<TYPE>& v)
{
for (int j = 0; j < v.m_size; j++)
{
}
template <typename T>
-vector<T> operator +(const vector<T> v1, const vector<T> v2)
+vect<T> operator +(const vect<T> v1, const vect<T> v2)
{
assert(v1.m_size == v2.m_size);
- vector<T> v3(v1.m_size);
+ vect<T> v3(v1.m_size);
for (int j = 0; j < v1.m_size; j++)
v3.m_vec[j] = v1.m_vec[j] + v2.m_vec[j];
}
template <typename T>
-vector<T> operator +(const vector<T> v, const T val)
+vect<T> operator +(const vect<T> v, const T val)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
for (int j = 0; j < v.m_size; j++)
v1.m_vec[j] = v.m_vec[j] + val;
}
template <typename T>
-vector<T> operator -(const vector<T> v1, const vector<T> v2)
+vect<T> operator -(const vect<T> v1, const vect<T> v2)
{
assert(v1.m_size == v2.m_size);
- vector<T> v3(v1.m_size);
+ vect<T> v3(v1.m_size);
for (int j = 0; j < v1.m_size; j++)
v3.m_vec[j] = v1.m_vec[j] - v2.m_vec[j];
}
template <typename T>
-vector<T> operator -(const vector<T> v, const T val)
+vect<T> operator -(const vect<T> v, const T val)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
for (int j = 0; j < v.m_size; j++)
v1.m_vec[j] = v.m_vec[j] - val;
}
template <typename T>
-matrix<T> operator *(const matrix<T> m, const vector<T> v)
+matrix<T> operator *(const matrix<T> m, const vect<T> v)
{
assert(m.m_rows == v.m_size);
assert(m.m_cols == 1);
}
template <typename T>
-vector<T> operator *(const vector<T> v, const matrix<T> m)
+vect<T> operator *(const vect<T> v, const matrix<T> m)
{
assert(m.m_rows == v.m_size);
assert(m.m_cols != 1);
- vector<T> v1(m.m_cols);
+ vect<T> v1(m.m_cols);
for (int j = 0; j < m.m_cols; j++)
{
}
template <typename T>
-vector<T> operator *(const vector<T> v, const T val)
+vect<T> operator *(const vect<T> v, const T val)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
for (int j = 0; j < v.m_size; j++)
v1.m_vec[j] = v.m_vec[j] * val;
}
template <typename T>
-vector<T> operator /(const vector<T> v, const T val)
+vect<T> operator /(const vect<T> v, const T val)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
for (int j = 0; j < v.m_size; j++)
v1.m_vec[j] = v.m_vec[j] / val;
}
template <typename T>
-bool operator ==(const vector<T> v1, const vector<T> v2)
+bool operator ==(const vect<T> v1, const vect<T> v2)
{
if (v1.m_size == v2.m_size)
{
}
template <typename T>
-bool operator !=(const vector<T> v1, const vector<T> v2)
+bool operator !=(const vect<T> v1, const vect<T> v2)
{
return (!(v1 == v2));
}
template <typename T>
-matrix<T> transpose(const vector<T> v)
+matrix<T> transpose(const vect<T> v)
{
matrix<T> m(v.m_size, 1);
}
template <typename T>
-vector<T> transpose(const matrix<T> m)
+vect<T> transpose(const matrix<T> m)
{
- vector<T> v(m.m_rows);
+ vect<T> v(m.m_rows);
for (int i = 0; i < m.m_rows; i++)
v.m_vec[i] = m.m_mat[i][0];
}
template <typename T>
-T mul(const vector<T> v, const matrix<T> m)
+T mul(const vect<T> v, const matrix<T> m)
{
assert(m.m_rows == v.m_size);
assert(m.m_cols == 1);
template <typename T>
-void insert_end(vector<T>& v, T val)
+void insert_end(vect<T>& v, T val)
{
for (int i = 0; i < (v.m_size - 1); i++)
v.m_vec[i] = v.m_vec[i+1];
}
template <typename T>
-vector<T> cross(const vector<T> v1, const vector<T> v2)
+vect<T> cross(const vect<T> v1, const vect<T> v2)
{
- vector<T> v3(v1.m_size);
+ vect<T> v3(v1.m_size);
v3.m_vec[0] = ((v1.m_vec[1] * v2.m_vec[2]) - (v1.m_vec[2] * v2.m_vec[1]));
v3.m_vec[1] = ((v1.m_vec[2] * v2.m_vec[0]) - (v1.m_vec[0] * v2.m_vec[2]));
}
template <typename T>
-bool is_initialized(const vector<T> v)
+bool is_initialized(const vect<T> v)
{
- vector<T> v1(v.m_size);
+ vect<T> v1(v.m_size);
bool retval;
retval = (v == v1) ? false : true;
}
template <typename T>
-T var(const vector<T> v)
+T var(const vect<T> v)
{
T val = 0;
T mean, var, diff;
#include "matrix.h"
template <typename TYPE>
-class vector {
+class vect {
public:
int m_size;
TYPE *m_vec;
- vector(void);
- vector(const int size);
- vector(const int size, TYPE *vec_data);
- vector(const vector<TYPE>& v);
- ~vector();
+ vect(void);
+ vect(const int size);
+ vect(const int size, TYPE *vec_data);
+ vect(const vect<TYPE>& v);
+ ~vect();
- vector<TYPE> operator =(const vector<TYPE>& v);
+ vect<TYPE> operator =(const vect<TYPE>& v);
template<typename T> friend ostream& operator << (ostream& dout,
- vector<T>& v);
- template<typename T> friend vector<T> operator +(const vector<T> v1,
- const vector<T> v2);
- template<typename T> friend vector<T> operator +(const vector<T> v,
+ vect<T>& v);
+ template<typename T> friend vect<T> operator +(const vect<T> v1,
+ const vect<T> v2);
+ template<typename T> friend vect<T> operator +(const vect<T> v,
const T val);
- template<typename T> friend vector<T> operator -(const vector<T> v1,
- const vector<T> v2);
- template<typename T> friend vector<T> operator -(const vector<T> v,
+ template<typename T> friend vect<T> operator -(const vect<T> v1,
+ const vect<T> v2);
+ template<typename T> friend vect<T> operator -(const vect<T> v,
const T val);
template<typename T> friend matrix<T> operator *(const matrix<T> v1,
- const vector<T> v2);
- template<typename T> friend vector<T> operator *(const vector<T> v,
+ const vect<T> v2);
+ template<typename T> friend vect<T> operator *(const vect<T> v,
const matrix<T> m);
- template<typename T> friend vector<T> operator *(const vector<T> v,
+ template<typename T> friend vect<T> operator *(const vect<T> v,
const T val);
- template<typename T> friend vector<T> operator /(const vector<T> v1,
+ template<typename T> friend vect<T> operator /(const vect<T> v1,
const T val);
- template<typename T> friend bool operator ==(const vector<T> v1,
- const vector<T> v2);
- template<typename T> friend bool operator !=(const vector<T> v1,
- const vector<T> v2);
+ template<typename T> friend bool operator ==(const vect<T> v1,
+ const vect<T> v2);
+ template<typename T> friend bool operator !=(const vect<T> v1,
+ const vect<T> v2);
- template<typename T> friend T mul(const vector<T> v, const matrix<T> m);
- template<typename T> friend void insert_end(vector<T>& v, T val);
- template<typename T> friend matrix<T> transpose(const vector<T> v);
- template <typename T> friend vector<T> transpose(const matrix<T> m);
- template<typename T> friend vector<T> cross(const vector<T> v1,
- const vector<T> v2);
- template <typename T> friend T var(const vector<T> v);
- template <typename T> friend bool is_initialized(const vector<T> v);
+ template<typename T> friend T mul(const vect<T> v, const matrix<T> m);
+ template<typename T> friend void insert_end(vect<T>& v, T val);
+ template<typename T> friend matrix<T> transpose(const vect<T> v);
+ template <typename T> friend vect<T> transpose(const matrix<T> m);
+ template<typename T> friend vect<T> cross(const vect<T> v1,
+ const vect<T> v2);
+ template <typename T> friend T var(const vect<T> v);
+ template <typename T> friend bool is_initialized(const vect<T> v);
};
#include "vector.cpp"