This is one of the four patches being submitted to merge the changes made to devel/tizen to tizen branch.
This contains the feature where the template structure of the vector and matrix classes was changed.
Patch Set 3: Changes related to coding guidelines as per the comments
Build has been checked for armv7l and aarch64 for public repo.
Change-Id: Ia26d66b1ca351a22de92b858f6b60dbfee242d42
#include <math.h>
-#define EULER_SIZE 3
#define RAD2DEG 57.2957795
#define DEG2RAD 0.0174532925
template <typename TYPE>
-euler_angles<TYPE>::euler_angles() : m_ang(EULER_SIZE)
+euler_angles<TYPE>::euler_angles() : m_ang()
{
}
{
TYPE euler_data[EULER_SIZE] = {roll, pitch, azimuth};
- vect<TYPE> v(EULER_SIZE, euler_data);
+ vect<TYPE, EULER_SIZE> v(euler_data);
m_ang = v;
}
template <typename TYPE>
-euler_angles<TYPE>::euler_angles(const vect<TYPE> v)
+euler_angles<TYPE>::euler_angles(const vect<TYPE, EULER_SIZE> v)
{
m_ang = v;
}
template <typename T>
euler_angles<T> rad2deg(const euler_angles<T> e)
{
- euler_angles<T> result(e.m_ang * (T) RAD2DEG);
- return result;
+ return (e.m_ang * (T) RAD2DEG);
}
template <typename T>
euler_angles<T> deg2rad(const euler_angles<T> e)
{
- euler_angles<T> result(e.m_ang * (T) DEG2RAD);
- return result;
+ return (e.m_ang * (T) DEG2RAD);
+}
+
+template<typename T>
+quaternion<T> euler2quat(euler_angles<T> euler) {
+ T theta = euler.m_ang.m_vec[0];
+ T phi = euler.m_ang.m_vec[1];
+ T psi = euler.m_ang.m_vec[2];
+
+ T R[ROT_MAT_ROWS][ROT_MAT_COLS];
+ R[0][0] = cos(psi)*cos(theta);
+ R[0][1] = -sin(psi)*cos(phi) + cos(psi)*sin(theta)*sin(phi);
+ R[0][2] = sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi);
+ R[1][0] = sin(psi)*cos(theta);
+ R[1][1] = cos(psi)*cos(phi) + sin(psi)*sin(theta)*sin(phi);
+ R[1][2] = -cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi);
+ R[2][0] = -sin(theta);
+ R[2][1] = cos(theta)*sin(phi);
+ R[2][2] = cos(theta)*cos(phi);
+
+ rotation_matrix<T> rot_mat(R);
+ quaternion<T> q = rot_mat2quat(rot_mat);
+ return q;
}
#endif //_EULER_ANGLES_H_
#include "vector.h"
#include "quaternion.h"
+#include "rotation_matrix.h"
+
+#define EULER_SIZE 3
template <typename TYPE>
class euler_angles {
public:
- vect<TYPE> m_ang;
+ vect<TYPE,EULER_SIZE> m_ang;
euler_angles();
euler_angles(const TYPE roll, const TYPE pitch, const TYPE azimuth);
- euler_angles(const vect<TYPE> v);
+ euler_angles(const vect<TYPE, EULER_SIZE> v);
euler_angles(const euler_angles<TYPE>& e);
~euler_angles();
euler_angles<TYPE> operator =(const euler_angles<TYPE>& e);
template<typename T> friend euler_angles<T> quat2euler(const quaternion<T> q);
+ template<typename T> friend quaternion<T> euler2quat(euler_angles<T> euler);
template<typename T> friend euler_angles<T> rad2deg(const euler_angles<T> e);
template<typename T> friend euler_angles<T> deg2rad(const euler_angles<T> e);
};
#ifdef _MATRIX_H_
-template <typename TYPE>
-matrix<TYPE>::matrix(void)
+TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(void)
{
- m_mat = NULL;
+ for (int i = 0; i < ROW; i++)
+ for (int j = 0; j < COL; j++)
+ m_mat[i][j] = 0;
}
-template <typename TYPE>
-matrix<TYPE>::matrix(const int rows, const int cols)
+TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(const matrix<TYPE, ROW, COL>& m)
{
- m_rows = rows;
- m_cols = cols;
- m_mat = new TYPE *[m_rows];
-
- for (int i = 0; i < m_rows; i++)
- m_mat[i] = new TYPE [m_cols]();
-}
-
-template <typename TYPE>
-matrix<TYPE>::matrix(const matrix<TYPE>& m)
-{
- m_rows = m.m_rows;
- m_cols = m.m_cols;
- m_mat = new TYPE *[m_rows];
-
- for (int i = 0; i < m_rows; i++)
- m_mat[i] = new TYPE [m_cols];
-
- for (int p = 0; p < m_rows; p++)
- for (int q = 0; q < m_cols; q++)
+ for (int p = 0; p < ROW; p++)
+ for (int q = 0; q < COL; q++)
m_mat[p][q] = m.m_mat[p][q];
}
-template <typename TYPE>
-matrix<TYPE>::matrix(const int rows, const int cols, TYPE *mat_data)
+TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(TYPE mat_data[ROW][COL])
{
- m_rows = rows;
- m_cols = cols;
- m_mat = new TYPE *[m_rows];
-
- for (int i = 0; i < m_rows; i++)
- m_mat[i] = new TYPE [m_cols];
-
- for (int i = 0; i < m_rows; i++)
- for (int j = 0; j < m_cols; j++)
- m_mat[i][j] = *mat_data++;
+ for (int i = 0; i < ROW; i++)
+ for (int j = 0; j < COL; j++)
+ m_mat[i][j] = mat_data[i][j];
}
-template <typename TYPE>
-matrix<TYPE>::~matrix()
+TYPE_ROW_COL matrix<TYPE, ROW, COL>::~matrix()
{
- if (m_mat != NULL)
- {
- for (int i = 0; i < m_rows; i++)
- delete[] m_mat[i];
- delete[] m_mat;
- }
}
-template <typename TYPE>
-matrix<TYPE> matrix<TYPE>::operator =(const matrix<TYPE>& m)
+TYPE_ROW_COL matrix<TYPE, ROW, COL> matrix<TYPE, ROW, COL>::operator =(const matrix<TYPE, ROW, COL>& m)
{
if (this == &m)
{
return *this;
}
- if (m_mat == NULL)
- {
- m_rows = m.m_rows;
- m_cols = m.m_cols;
- m_mat = new TYPE *[m_rows];
-
- for (int i = 0; i < m_rows; i++)
- m_mat[i] = new TYPE [m_cols];
- }
- else
- {
- if ((m_rows != m.m_rows) || (m_cols != m.m_cols))
- {
- for (int i = 0; i < m_rows; i++)
- delete[] m_mat[i];
- delete[] m_mat;
-
- m_rows = m.m_rows;
- m_cols = m.m_cols;
- m_mat = new TYPE *[m_rows];
-
- for (int i = 0; i < m_rows; i++)
- m_mat[i] = new TYPE [m_cols];
- }
- }
-
- for (int p = 0; p < m_rows; p++)
- for (int q = 0; q < m_cols; q++)
+ for (int p = 0; p < ROW; p++)
+ for (int q = 0; q < COL; q++)
m_mat[p][q] = m.m_mat[p][q];
return *this;
}
-template <typename T>
-ostream& operator <<(ostream& dout, matrix<T>& m)
+T_R_C ostream& operator <<(ostream& dout, matrix<T, R, C>& m)
{
- for (int i = 0; i < m.m_rows; i++)
+ for (int i = 0; i < R; i++)
{
- for (int j = 0; j < m.m_cols; j++)
+ for (int j = 0; j < C; j++)
{
dout << m.m_mat[i][j] << "\t";
}
return dout;
}
-template <typename T>
-matrix<T> operator +(const matrix<T> m1, const matrix<T> m2)
+T_R_C matrix<T, R, C> operator +(const matrix<T, R, C> m1, const matrix<T, R, C> m2)
{
- assert(m1.m_rows == m2.m_rows);
- assert(m1.m_cols == m2.m_cols);
-
- matrix<T> m3(m1.m_rows, m1.m_cols);
+ matrix<T, R, C> m3;
- for (int i = 0; i < m1.m_rows; i++)
- for (int j = 0; j < m1.m_cols; j++)
+ for (int i = 0; i < R; i++)
+ for (int j = 0; j < C; j++)
m3.m_mat[i][j] = m1.m_mat[i][j] + m2.m_mat[i][j];
return m3;
}
-template <typename T>
-matrix<T> operator +(const matrix<T> m, const T val)
+T_R_C matrix<T, R, C> operator +(const matrix<T, R, C> m, const T val)
{
- matrix<T> m1(m.m_rows, m.m_cols);
+ matrix<T, R, C> m1;
- for (int i = 0; i < m.m_rows; i++)
- for (int j = 0; j < m.m_cols; j++)
+ for (int i = 0; i < R; i++)
+ for (int j = 0; j < C; j++)
m1.m_mat[i][j] = m.m_mat[i][j] + val;
return m1;
}
-template <typename T>
-matrix<T> operator -(const matrix<T> m1, const matrix<T> m2)
+T_R_C matrix<T, R, C> operator -(const matrix<T, R, C> m1, const matrix<T, R, C> m2)
{
- assert(m1.m_rows == m2.m_rows);
- assert(m1.m_cols == m2.m_cols);
+ matrix<T, R, C> m3;
- matrix<T> m3(m1.m_rows, m1.m_cols);
-
- for (int i = 0; i < m1.m_rows; i++)
- for (int j = 0; j < m1.m_cols; j++)
+ for (int i = 0; i < R; i++)
+ for (int j = 0; j < C; j++)
m3.m_mat[i][j] = m1.m_mat[i][j] - m2.m_mat[i][j];
return m3;
}
-template <typename T>
-matrix<T> operator -(const matrix<T> m, const T val)
+T_R_C matrix<T, R, C> operator -(const matrix<T, R, C> m, const T val)
{
- matrix<T> m1(m.m_rows, m.m_cols);
+ matrix<T, R, C> m1;
- for (int i = 0; i < m.m_rows; i++)
- for (int j = 0; j < m.m_cols; j++)
+ for (int i = 0; i < R; i++)
+ for (int j = 0; j < C; j++)
m1.m_mat[i][j] = m.m_mat[i][j] - val;
return m1;
}
-template <typename T>
-matrix<T> operator *(const matrix<T> m1, const matrix<T> m2)
+T_R_C_C2 matrix<T, R, C2> operator *(const matrix<T, R, C> m1, const matrix<T, C, C2> m2)
{
- assert(m1.m_rows == m2.m_cols);
- assert(m1.m_cols == m2.m_rows);
-
- matrix<T> m3(m1.m_rows, m2.m_cols);
+ matrix<T, R, C2> m3;
- for (int i = 0; i < m1.m_rows; i++)
+ for (int i = 0; i < R; i++)
{
- for (int j = 0; j < m2.m_cols; j++)
+ for (int j = 0; j < C2; j++)
{
m3.m_mat[i][j] = 0;
- for (int k = 0; k < m2.m_rows; k++)
+ for (int k = 0; k < C; k++)
m3.m_mat[i][j] += m1.m_mat[i][k] * m2.m_mat[k][j];
}
}
return m3;
}
-template <typename T>
-matrix<T> operator *(const matrix<T> m, const T val)
+T_R_C matrix<T, R, C> operator *(const matrix<T, R, C> m, const T val)
{
- matrix<T> m1(m.m_rows, m.m_cols);
+ matrix<T, R, C> m1;
- for (int i = 0; i < m.m_rows; i++)
- for (int j = 0; j < m.m_cols; j++)
+ for (int i = 0; i < R; i++)
+ for (int j = 0; j < C; j++)
m1.m_mat[i][j] = m.m_mat[i][j] * val;
return m1;
}
-template <typename T>
-matrix<T> operator /(const matrix<T> m1, const T val)
+T_R_C matrix<T, R, C> operator /(const matrix<T, R, C> m1, const T val)
{
- matrix<T> m3(m1.m_rows, m1.m_cols);
+ matrix<T, R, C> m3;
- for (int i = 0; i < m1.m_rows; i++)
- for (int j = 0; j < m1.m_cols; j++)
+ for (int i = 0; i < R; i++)
+ for (int j = 0; j < C; j++)
m3.m_mat[i][j] = m1.m_mat[i][j] / val;
return m3;
}
-template <typename T>
-bool operator ==(const matrix<T> m1, const matrix<T> m2)
+T_R1_C1_R2_C2 bool operator ==(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2)
{
- if ((m1.m_rows == m2.m_rows) && (m1.m_cols == m2.m_cols))
+ if ((R1 == R2) && (C1 == C2))
{
- for (int i = 0; i < m1.m_rows; i++)
- for (int j = 0; j < m2.m_cols; j++)
+ for (int i = 0; i < R1; i++)
+ for (int j = 0; j < C2; j++)
if (m1.m_mat[i][j] != m2.m_mat[i][j])
return false;
}
return true;
}
-template <typename T>
-bool operator !=(const matrix<T> m1, const matrix<T> m2)
+T_R1_C1_R2_C2 bool operator !=(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2)
{
return (!(m1 == m2));
}
-template <typename T>
-matrix<T> tran(const matrix<T> m)
+T_R_C matrix<T, R, C> tran(const matrix<T, R, C> m)
{
- matrix<T> m1(m.m_cols, m.m_rows);
+ matrix<T, R, C> m1;
- for (int i = 0; i < m.m_rows; i++)
- for (int j = 0; j < m.m_cols; j++)
+ for (int i = 0; i < R; i++)
+ for (int j = 0; j < C; j++)
m1.m_mat[j][i] = m.m_mat[i][j];
return m1;
}
-
-template <typename T>
-matrix<T> mul(const matrix<T> m1, const matrix<T> m2)
-{
- assert(m2.m_cols == 1);
- assert(m1.m_cols == m2.m_rows);
-
- matrix<T> m3(m1.m_rows, 1);
-
- for (int i = 0; i < m1.m_rows; i++)
- {
- m3.m_mat[i][0] = 0;
- for (int k = 0; k < m2.m_rows; k++)
- m3.m_mat[i][0] += m1.m_mat[i][k] * m2.m_mat[k][0];
- }
-
- return m3;
-}
-
#endif //_MATRIX_H_
#include <iostream>
using namespace std;
-template <typename TYPE>
-class matrix {
+#define TYPE_ROW_COL template<typename TYPE, int ROW, int COL>
+#define T_R_C template<typename T, int R, int C>
+#define T_R_C_C2 template<typename T, int R, int C, int C2>
+#define T_R1_C1_R2_C2 template<typename T, int R1, int C1, int R2, int C2>
+
+TYPE_ROW_COL class matrix {
public:
- int m_rows;
- int m_cols;
- TYPE **m_mat;
+ TYPE m_mat[ROW][COL];
matrix(void);
- matrix(const int rows, const int cols);
- matrix(const int rows, const int cols, TYPE *mat_data);
- matrix(const matrix<TYPE>& m);
+ matrix(TYPE mat_data[ROW][COL]);
+ matrix(const matrix<TYPE, ROW, COL>& m);
~matrix();
- matrix<TYPE> operator =(const matrix<TYPE>& m);
-
- template<typename T> friend ostream& operator << (ostream& dout,
- matrix<T>& m);
- template<typename T> friend matrix<T> operator +(const matrix<T> m1,
- const matrix<T> m2);
- template<typename T> friend matrix<T> operator +(const matrix<T> m,
- const T val);
- template<typename T> friend matrix<T> operator -(const matrix<T> m1,
- const matrix<T> m2);
- template<typename T> friend matrix<T> operator -(const matrix<T> m,
- const T val);
- template<typename T> friend matrix<T> operator *(const matrix<T> m1,
- const matrix<T> m2);
- template<typename T> friend matrix<T> operator *(const matrix<T> m,
- const T val);
- template<typename T> friend matrix<T> operator /(const matrix<T> m1,
- const T val);
- template<typename T> friend bool operator ==(const matrix<T> m1,
- const matrix<T> m2);
- template<typename T> friend bool operator !=(const matrix<T> m1,
- const matrix<T> m2);
-
- template<typename T> friend matrix<T> tran(const matrix<T> m);
- template <typename T> friend matrix<T> mul(const matrix<T> m1,
- const matrix<T> m2);
+ matrix<TYPE, ROW, COL> operator =(const matrix<TYPE, ROW, COL>& m);
+
+ T_R_C friend ostream& operator << (ostream& dout, matrix<T, R, C>& m);
+ T_R_C friend matrix<T, R, C> operator +(const matrix<T, R, C> m1, const matrix<T, R, C> m2);
+ T_R_C friend matrix<T, R, C> operator +(const matrix<T, R, C> m, const T val);
+ T_R_C friend matrix<T, R, C> operator -(const matrix<T, R, C> m1, const matrix<T, R, C> m2);
+ T_R_C friend matrix<T, R, C> operator -(const matrix<T, R, C> m, const T val);
+ T_R_C_C2 friend matrix<T, R, C2> operator *(const matrix<T, R, C> m1, const matrix<T, C, C2> m2);
+ T_R_C friend matrix<T, R, C> operator *(const matrix<T, R, C> m, const T val);
+ T_R_C friend matrix<T, R, C> operator /(const matrix<T, R, C> m1, const T val);
+ T_R1_C1_R2_C2 friend bool operator ==(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2);
+ T_R1_C1_R2_C2 friend bool operator !=(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2);
+ T_R_C friend matrix<T, R, C> tran(const matrix<T, R, C> m);
};
#include "matrix.cpp"
#define ABS(val) (((val) < 0) ? -(val) : (val))
-// M-matrix, V-vector, MxN=> matrix dimension, R-RowCount, C-Column count
-#define M3X3R 3
-#define M3X3C 3
-#define M6X6R 6
-#define M6X6C 6
-#define V1x3S 3
-#define V1x4S 4
-#define V1x6S 6
template <typename TYPE>
orientation_filter<TYPE>::orientation_filter()
std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL);
- vect<TYPE> vec(MOVING_AVERAGE_WINDOW_LENGTH, arr);
- vect<TYPE> vec1x3(V1x3S);
- vect<TYPE> vec1x6(V1x6S);
- matrix<TYPE> mat6x6(M6X6R, M6X6C);
+ vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> vec(arr);
m_var_gyr_x = vec;
m_var_gyr_y = vec;
m_var_pitch = vec;
m_var_azimuth = vec;
- m_tran_mat = mat6x6;
- m_measure_mat = mat6x6;
- m_pred_cov = mat6x6;
- m_driv_cov = mat6x6;
- m_aid_cov = mat6x6;
-
- m_bias_correction = vec1x3;
- m_state_new = vec1x6;
- m_state_old = vec1x6;
- m_state_error = vec1x6;
-
m_pitch_phase_compensation = 1;
m_roll_phase_compensation = 1;
m_azimuth_phase_compensation = 1;
}
template <typename TYPE>
-inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<TYPE> accel,
+inline void orientation_filter<TYPE>::init_accel_gyro_mag_data(const sensor_data<TYPE> accel,
const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
{
unsigned long long sample_interval_gyro = SAMPLE_INTV;
m_accel.m_data = accel.m_data;
- m_gyro.m_data = gyro.m_data;
m_magnetic.m_data = magnetic.m_data;
if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0)
m_gyro.m_time_stamp = gyro.m_time_stamp;
m_magnetic.m_time_stamp = magnetic.m_time_stamp;
- m_gyro.m_data = m_gyro.m_data - m_bias_correction;
+ m_gyro.m_data = gyro.m_data - m_bias_correction;
}
template <typename TYPE>
-inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<TYPE> accel,
+inline void orientation_filter<TYPE>::init_accel_mag_data(const sensor_data<TYPE> accel,
const sensor_data<TYPE> magnetic)
{
m_accel.m_data = accel.m_data;
}
template <typename TYPE>
+inline void orientation_filter<TYPE>::init_accel_gyro_data(const sensor_data<TYPE> accel,
+ const sensor_data<TYPE> gyro)
+{
+ unsigned long long sample_interval_gyro = SAMPLE_INTV;
+
+ m_accel.m_data = accel.m_data;
+
+ if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0)
+ sample_interval_gyro = gyro.m_time_stamp - m_gyro.m_time_stamp;
+
+ m_gyro_dt = sample_interval_gyro * US2S;
+
+ m_accel.m_time_stamp = accel.m_time_stamp;
+ m_gyro.m_time_stamp = gyro.m_time_stamp;
+
+ m_gyro.m_data = gyro.m_data - m_bias_correction;
+}
+
+template <typename TYPE>
inline void orientation_filter<TYPE>::orientation_triad_algorithm()
{
TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
TYPE arr_mag_e[V1x3S] = {0.0, (TYPE) m_magnetic_alignment_factor, 0.0};
- vect<TYPE> acc_e(V1x3S, arr_acc_e);
- vect<TYPE> mag_e(V1x3S, arr_mag_e);
+ vect<TYPE, V1x3S> acc_e(arr_acc_e);
+ vect<TYPE, V1x3S> mag_e(arr_mag_e);
- vect<TYPE> acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data);
- vect<TYPE> acc_e_x_mag_e = cross(acc_e, mag_e);
+ vect<TYPE, SENSOR_DATA_SIZE> acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data);
+ vect<TYPE, V1x3S> acc_e_x_mag_e = cross(acc_e, mag_e);
- vect<TYPE> cross1 = cross(acc_b_x_mag_b, m_accel.m_data);
- vect<TYPE> cross2 = cross(acc_e_x_mag_e, acc_e);
+ vect<TYPE, SENSOR_DATA_SIZE> cross1 = cross(acc_b_x_mag_b, m_accel.m_data);
+ vect<TYPE, V1x3S> cross2 = cross(acc_e_x_mag_e, acc_e);
- matrix<TYPE> mat_b(M3X3R, M3X3C);
- matrix<TYPE> mat_e(M3X3R, M3X3C);
+ matrix<TYPE, M3X3R, M3X3C> mat_b;
+ matrix<TYPE, M3X3R, M3X3C> mat_e;
for(int i = 0; i < M3X3R; i++)
{
mat_e.m_mat[i][2] = cross2.m_vec[i];
}
- matrix<TYPE> mat_b_t = tran(mat_b);
+ matrix<TYPE, M3X3R, M3X3C> mat_b_t = tran(mat_b);
rotation_matrix<TYPE> rot_mat(mat_e * mat_b_t);
m_quat_aid = rot_mat2quat(rot_mat);
}
template <typename TYPE>
+inline void orientation_filter<TYPE>::compute_accel_orientation()
+{
+ TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
+
+ vect<TYPE, V1x3S> acc_e(arr_acc_e);
+
+ m_quat_aid = sensor_data2quat(m_accel, acc_e);
+}
+
+template <typename TYPE>
inline void orientation_filter<TYPE>::compute_covariance()
{
TYPE var_gyr_x, var_gyr_y, var_gyr_z;
m_measure_mat.m_mat[2][2] = 1;
if (is_initialized(m_state_old))
- m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old)));
+ m_state_new = transpose(m_tran_mat * transpose(m_state_old));
m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
- for (int j=0; j<M6X6C; ++j) {
- for (int i=0; i<M6X6R; ++i) {
- if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
- m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
- }
- if (ABS(m_state_new.m_vec[j]) < NEGLIGIBLE_VAL)
- m_state_new.m_vec[j] = NEGLIGIBLE_VAL;
- }
-
if(!is_initialized(m_quat_driv.m_quat))
m_quat_driv = m_quat_aid;
template <typename TYPE>
inline void orientation_filter<TYPE>::measurement_update()
{
- matrix<TYPE> gain(M6X6R, M6X6C);
- matrix<TYPE> iden(M6X6R, M6X6C);
- iden.m_mat[0][0] = iden.m_mat[1][1] = iden.m_mat[2][2] = 1;
- iden.m_mat[3][3] = iden.m_mat[4][4] = iden.m_mat[5][5] = 1;
+ matrix<TYPE, M6X6R, M6X6C> gain;
+ TYPE iden = 0;
for (int j=0; j<M6X6C; ++j) {
- for (int i=0; i<M6X6R; ++i) {
+ for (int i=0; i<M6X6R; ++i) {
gain.m_mat[i][j] = m_pred_cov.m_mat[j][i] / (m_pred_cov.m_mat[j][j] + m_aid_cov.m_mat[j][j]);
- m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
- }
- matrix<TYPE> temp = iden;
+ m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
- for (int i=0; i<M6X6R; ++i)
- temp.m_mat[i][j] = iden.m_mat[i][j] - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i]);
+ if (i == j)
+ iden = 1;
+ else
+ iden = 0;
- m_pred_cov = temp * m_pred_cov;
- }
+ m_pred_cov.m_mat[j][i] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) *
+ m_pred_cov.m_mat[j][i];
- for (int j=0; j<M6X6C; ++j) {
- for (int i=0; i<M6X6R; ++i) {
- if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
- m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
+ if (ABS(m_pred_cov.m_mat[j][i]) < NEGLIGIBLE_VAL)
+ m_pred_cov.m_mat[j][i] = NEGLIGIBLE_VAL;
}
}
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]};
- vect<TYPE> vec(V1x3S, arr_bias);
+ vect<TYPE, V1x3S> vec(arr_bias);
m_bias_correction = vec;
}
{
euler_angles<TYPE> cor_euler_ang;
- initialize_sensor_data(accel, gyro, magnetic);
+ init_accel_gyro_mag_data(accel, gyro, magnetic);
normalize(m_accel);
m_gyro.m_data = m_gyro.m_data * (TYPE) PI;
quaternion<TYPE> orientation_filter<TYPE>::get_geomagnetic_quaternion(const sensor_data<TYPE> accel,
const sensor_data<TYPE> magnetic)
{
- initialize_sensor_data(accel, magnetic);
+ init_accel_mag_data(accel, magnetic);
normalize(m_accel);
normalize(m_magnetic);
#include "euler_angles.h"
#include "rotation_matrix.h"
+#define MOVING_AVERAGE_WINDOW_LENGTH 20
+
+#define M3X3R 3
+#define M3X3C 3
+#define M6X6R 6
+#define M6X6C 6
+#define V1x3S 3
+#define V1x4S 4
+#define V1x6S 6
+
template <typename TYPE>
class orientation_filter {
public:
sensor_data<TYPE> m_accel;
sensor_data<TYPE> m_gyro;
sensor_data<TYPE> m_magnetic;
- 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_azimuth;
- 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;
- vect<TYPE> m_state_new;
- vect<TYPE> m_state_old;
- vect<TYPE> m_state_error;
- vect<TYPE> m_bias_correction;
+ vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_x;
+ vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_y;
+ vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_z;
+ vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_roll;
+ vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_pitch;
+ vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_azimuth;
+ matrix<TYPE, M6X6R, M6X6C> m_driv_cov;
+ matrix<TYPE, M6X6R, M6X6C> m_aid_cov;
+ matrix<TYPE, M6X6R, M6X6C> m_tran_mat;
+ matrix<TYPE, M6X6R, M6X6C> m_measure_mat;
+ matrix<TYPE, M6X6R, M6X6C> m_pred_cov;
+ vect<TYPE, V1x6S> m_state_new;
+ vect<TYPE, V1x6S> m_state_old;
+ vect<TYPE, V1x6S> m_state_error;
+ vect<TYPE, V1x3S> m_bias_correction;
quaternion<TYPE> m_quat_aid;
quaternion<TYPE> m_quat_driv;
rotation_matrix<TYPE> m_rot_matrix;
orientation_filter();
~orientation_filter();
- inline void initialize_sensor_data(const sensor_data<TYPE> accel,
+ inline void init_accel_gyro_mag_data(const sensor_data<TYPE> accel,
const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
- inline void initialize_sensor_data(const sensor_data<TYPE> accel,
+ inline void init_accel_mag_data(const sensor_data<TYPE> accel,
const sensor_data<TYPE> magnetic);
+ inline void init_accel_gyro_data(const sensor_data<TYPE> accel,
+ const sensor_data<TYPE> gyro);
inline void orientation_triad_algorithm();
+ inline void compute_accel_orientation();
inline void compute_covariance();
inline void time_update();
inline void measurement_update();
#include <math.h>
-#define QUAT_SIZE 4
-
template <typename T> int sgn(T val) {
if (val >= 0)
return 1;
}
template <typename TYPE>
-quaternion<TYPE>::quaternion() : m_quat(QUAT_SIZE)
+quaternion<TYPE>::quaternion() : m_quat()
{
}
{
TYPE vec_data[QUAT_SIZE] = {w, x, y, z};
- vect<TYPE> v(QUAT_SIZE, vec_data);
+ vect<TYPE, QUAT_SIZE> v(vec_data);
m_quat = v;
}
template <typename TYPE>
-quaternion<TYPE>::quaternion(const vect<TYPE> v)
+quaternion<TYPE>::quaternion(const vect<TYPE, QUAT_SIZE> v)
{
m_quat = v;
}
template <typename T>
quaternion<T> operator *(const quaternion<T> q, const T val)
{
- quaternion<T> res;
- res = q.m_quat * val;
- return (res);
+ return (q.m_quat * val);
}
template <typename T>
return q;
}
+
+template<typename T>
+quaternion<T> axis2quat(const vect<T, REF_VEC_SIZE> axis, const T angle)
+{
+ T w;
+ vect<T, REF_VEC_SIZE> imag;
+
+ w = cos(angle/2);
+ imag = axis * (T)(-sin(angle/2));
+
+ quaternion<T> q(w, imag.m_vec[0], imag.m_vec[1], imag.m_vec[2]);
+
+ return q;
+}
#endif //_QUATERNION_H_
#include "vector.h"
+#define QUAT_SIZE 4
+#define REF_VEC_SIZE 3
+
template <typename TYPE>
class quaternion {
public:
- vect<TYPE> m_quat;
+ vect<TYPE, QUAT_SIZE> m_quat;
quaternion();
quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z);
- quaternion(const vect<TYPE> v);
+ quaternion(const vect<TYPE, QUAT_SIZE> v);
quaternion(const quaternion<TYPE>& q);
~quaternion();
const quaternion<T> q2);
template<typename T> friend quaternion<T> phase_correction(const quaternion<T> q1,
const quaternion<T> q2);
+ template<typename T> friend quaternion<T> axis2quat(const vect<T, REF_VEC_SIZE> axis,
+ const T angle);
};
#include "quaternion.cpp"
#if defined (_ROTATION_MATRIX_H_) && defined (_MATRIX_H_)
#define QUAT_LEN 4
-#define ROT_MAT_ROWS 3
-#define ROT_MAT_COLS 3
template <typename T> T get_sign(T val)
{
}
template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix() : m_rot_mat(ROT_MAT_ROWS, ROT_MAT_COLS)
+rotation_matrix<TYPE>::rotation_matrix() : m_rot_mat()
{
}
template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix(const matrix<TYPE> m)
+rotation_matrix<TYPE>::rotation_matrix(const matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m)
{
m_rot_mat = m;
}
template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix(const int rows, const int cols, TYPE *mat_data)
+rotation_matrix<TYPE>::rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS])
{
- matrix<TYPE> m(rows, cols, mat_data);
+ matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m(mat_data);
m_rot_mat = m;
}
R[2][1] = 2 * ((y * z) - (w * x));
R[2][2] = (2 * w * w) - 1 + (2 * z * z);
- rotation_matrix<T> rm(ROT_MAT_ROWS, ROT_MAT_COLS, &R[0][0]);
+ rotation_matrix<T> rm(R);
return rm;
}
#include "matrix.h"
#include "quaternion.h"
+#define ROT_MAT_ROWS 3
+#define ROT_MAT_COLS 3
+
template <typename TYPE>
class rotation_matrix {
public:
- matrix<TYPE> m_rot_mat;
+ matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m_rot_mat;
rotation_matrix();
- rotation_matrix(const matrix<TYPE> m);
- rotation_matrix(const int rows, const int cols, TYPE *mat_data);
+ rotation_matrix(const matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m);
+ rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS]);
rotation_matrix(const rotation_matrix<TYPE>& rm);
~rotation_matrix();
#include "math.h"
-#define SENSOR_DATA_SIZE 3
-
template <typename TYPE>
-sensor_data<TYPE>::sensor_data() : m_data(SENSOR_DATA_SIZE), m_time_stamp(0)
+sensor_data<TYPE>::sensor_data() : m_data(), m_time_stamp(0)
{
}
{
TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z};
- vect<TYPE> v(SENSOR_DATA_SIZE, vec_data);
+ vect<TYPE, SENSOR_DATA_SIZE> v(vec_data);
m_data = v;
m_time_stamp = time_stamp;
}
template <typename TYPE>
-sensor_data<TYPE>::sensor_data(const vect<TYPE> v,
+sensor_data<TYPE>::sensor_data(const vect<TYPE, SENSOR_DATA_SIZE> v,
const unsigned long long time_stamp)
{
m_data = v;
template <typename T>
sensor_data<T> operator +(sensor_data<T> data1, sensor_data<T> data2)
{
- sensor_data<T> result(data1.m_data + data2.m_data, data1.m_time_stamp);
+ sensor_data<T> result(data1.m_data + data2.m_data, 0);
return result;
}
return s;
}
+
+template<typename T>
+quaternion<T> sensor_data2quat(const sensor_data<T> data, const vect<T, REF_VEC_SIZE> ref_vec)
+{
+ vect<T, REF_VEC_SIZE> axis;
+ T angle;
+
+ axis = cross(data.m_data, ref_vec);
+ angle = acos(dot(data.m_data, ref_vec));
+
+ return axis2quat(axis, angle);
+}
+
#endif /* _SENSOR_DATA_H_ */
#define _SENSOR_DATA_H_
#include "vector.h"
+#include "quaternion.h"
+
+#define SENSOR_DATA_SIZE 3
template <typename TYPE>
class sensor_data {
public:
- vect<TYPE> m_data;
+ vect<TYPE, SENSOR_DATA_SIZE> 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 vect<TYPE> v,
+ sensor_data(const vect<TYPE, SENSOR_DATA_SIZE> v,
const unsigned long long time_stamp);
sensor_data(const sensor_data<TYPE>& s);
~sensor_data();
template<typename T> friend void normalize(sensor_data<T>& data);
template<typename T> friend sensor_data<T> scale_data(sensor_data<T> data,
T scaling_factor);
+ template<typename T> friend quaternion<T> sensor_data2quat(const sensor_data<T> data,
+ const vect<T, REF_VEC_SIZE> ref_vec);
};
#include "sensor_data.cpp"
float arr2[4] = {0.6, 0.6, -.18, -.44};
float arr3[4] = {-0.5, -0.36, .43, .03};
- vect<float> v0(3, arr0);
- vect<float> v1(3, arr1);
- vect<float> v2(4, arr2);
- vect<float> v3(4, arr3);
+ vect<float,3> v0(arr0);
+ vect<float,3> v1(arr1);
+ vect<float,4> v2(arr2);
+ vect<float,4> v3(arr3);
quaternion<float> q1(v2);
quaternion<float> q2(v3);
cout << "input\t" << q2.m_quat << "\n";
cout << "output\t" << e8.m_ang << "\n\n";
+ cout << "Euler to Quaternion\n";
+ quaternion<float> q3 = euler2quat(e8);
+ cout << "input\t" << e8.m_ang << "\n";
+ cout << "output\t" << q3.m_quat << "\n\n";
+
cout << "Radians to Degrees\n";
euler_angles<float> e6 = deg2rad(e0);
cout << "input\t" << e0.m_ang << "\n";
float arr8[3][3] = {{4.75, 0.65, 0.123}, {0.075, 5.302, 0.56}, {1.113, 0.475, 2.362}};
float arr9[3][3] = {{1, 2, 3}, {1, 2, 3}, {1, 2, 3}};
- matrix<float> m1(2, 2, (float *) arr0);
- matrix<float> m2(2, 2, (float *) arr1);
- matrix<float> m3(2, 2);
- matrix<float> m10(3, 3, (float *) arr3);
- matrix<float> m11(3, 2, (float *) arr5);
- matrix<float> m6(3, 3);
- matrix<float> m13(3, 2);
- matrix<float> m12(3, 3, (float *) arr4);
- matrix<float> m15(3, 3, (float *) arr8);
- matrix<float> m20(1, 3, (float *) arr11);
- matrix<float> m21(3, 1, (float *) arr12);
- matrix<float> m22(2, 3, (float *) arr15);
- matrix<float> m9(3, 3, (float *) arr9);
+ matrix<float, 2, 2> m1(arr0);
+ matrix<float, 2, 2> m2(arr1);
+ matrix<float, 2, 2> m3;
+ matrix<float, 3, 3> m10(arr3);
+ matrix<float, 3, 2> m11(arr5);
+ matrix<float, 3, 3> m6;
+ matrix<float, 3, 2> m13;
+ matrix<float, 3, 3> m12(arr4);
+ matrix<float, 3, 3> m15(arr8);
+ matrix<float, 1, 3> m20(arr11);
+ matrix<float, 3, 1> m21(arr12);
+ matrix<float, 2, 3> m22(arr15);
+ matrix<float, 3, 3> m9(arr9);
cout<< "Constructor Test\n";
cout<< "\n" << m6;
cout<< "\n\n\nMultiplication\n";
m6 = m10 * m12;
m3 = m1 * m2;
- matrix<float> m7(m20.m_rows, m21.m_cols);
+ matrix<float, 1, 1> m7;
m7 = m20 * m21;
cout<< "\n" << m10 << "\n" << m12;
cout<< "\nProduct:\n" << m6 << endl;
cout<< "\n" << m20 << "\n" << m21;
cout<< "\nProduct:\n" << m7 << endl;
cout<< "\n" << m9 << "\n" << m21;
- m21 = mul(m9, m21);
+ m21 = m9 * m21;
cout<< "\nProduct:\n" << m21 << endl;
cout<< "\n\n\nDivision\n";
cout<< "\n\nAssignment\n";
- m3 = m12;
+ matrix<float, 3, 3> m30 = m12;
cout<< "Input \n" << m12;
- cout<< "\nOutput:\n" << m3 << endl;
+ cout<< "\nOutput:\n" << m30 << endl;
}
{
float arr0[4] = {2344.98, 345.24, 456.12, 98.33};
float arr1[4] = {0.056, 0.34, -0.0076, 0.001};
+ float axis1[3] = {6.5, 7.5, 8.3};
+ float ang1 = 8.4;
- vect<float> v0(4, arr0);
- vect<float> v1(4, arr1);
+ vect<float, 4> v0(arr0);
+ vect<float, 4> v1(arr1);
+ vect<float, 3> v2(axis1);
quaternion<float> q0(v0);
quaternion<float> q1(v1);
cout << "input\t" << q1.m_quat << "\n";
q1.quat_normalize();
cout << "output\t" << q1.m_quat << "\n\n";
+
+ cout << "Axis2quat\n";
+ cout << "input\t" << " " << v2 << endl;
+ cout << endl;
+ quaternion<float> q11 = axis2quat(v2, ang1);
+ cout << "output\t" << q11.m_quat << "\n\n";
+ cout << endl;
+
}
float arr2[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}};
float arr3[3][3] = {{4.75, 0.65, 0.123}, {0.075, 5.302, 0.56}, {1.113, 0.475, 2.362}};
- matrix<float> m1(3, 3, (float *) arr1);
- matrix<float> m2(3, 3, (float *) arr2);
- matrix<float> m3(3, 3, (float *) arr3);
+ matrix<float, 3, 3> m1(arr1);
+ matrix<float, 3, 3> m2(arr2);
+ matrix<float, 3, 3> m3(arr3);
rotation_matrix<float> rm0, rm5;
rotation_matrix<float> rm1(m1);
rotation_matrix<float> rm2(m2);
rotation_matrix<float> rm3(m3);
- rotation_matrix<float> rm4(3, 3, (float *) arr1);
+ rotation_matrix<float> rm4(arr1);
quaternion<float> q0(-0.612372, 0.456436, 0.456436, 0.456436);
quaternion<float> q1;
int main()
{
float arr1[3] = {1.04, -4.678, -2.34};
-
- vect<float> v1(3, arr1);
+ float arr2[3] = {0, 0, 1};
+ vect<float,3> v1(arr1);
+ vect<float,3> v2(arr2);
sensor_data<float> sd1(2.0, 3.0, 4.0, 140737488355328);
sensor_data<float> sd2(1.04, -4.678, -2.34, 0);
sensor_data<float> sd3(0.054, 1.097, 4.456, 140737488355328);
+
sensor_data<float> sd10(v1, 140737488355328);
+ sensor_data<float> sd11(0.2, 0.1, 0.3, 140737488355328);
cout << "Constructor tests\n";
cout << "input\t" << v1 << "\n";
sensor_data<float> sd8 = scale_data(sd2, xx);
cout<< "\n" << sd2.m_data << "\n" << xx;
cout<< "\nResult:\n" << sd8.m_data << endl;
+
+ cout<<"\n\n\nConvert Sensor Data to Quaternion:\n";
+ quaternion<float> q = sensor_data2quat(sd11, v2);
+ cout<< "\n" << sd11.m_data << "\n" << v2;
+ cout<< "\nResult:\n" << q.m_quat << endl;
}
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};
- 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);
+ vect<float,5> v1(arr0);
+ vect<float,5> v2(arr8);
+ vect<float,4> v10(arr3);
+ vect<float,4> v12(arr4);
+ vect<float,6> v15(arr1);
+ vect<float,6> v20(arr43);
+ vect<float,3> v21(arr2);
+ vect<float,3> v22(arr15);
+ vect<float,5> v31(arr0);
+ vect<float,3> v3;
+ vect<float,2> vn;
+ vect<float,4> v61(arr4);
+ vect<float,3> v6;
+ vect<float,4> vm;
+ vect<float,5> v13;
+ vect<float,6> v95;
+ vect<float,5> v35(arr5);
+ vect<float,5> vl;
+ vect<float,4> vp;
+ vect<float,4> vr(arr4);
+ vect<float,3> vf;
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);
+ matrix<float,3 ,3> m12(arr57);
float arr67[3][1] = {{2.0}, {3.0}, {4.0}};
- matrix<float> m32(3, 1, (float *) arr67);
+ matrix<float,3,1> m32(arr67);
cout<< "Constructor Test\n";
cout<< "\n" << v3;
cout<< "\n\nAddition\n";
v3 = v21 + v22;
v95 = v15 + v20;
+ cout<< "\n\nNumbers added\n";
cout<< "\n" << v21 << "\n" << v22;
cout<< "\nSum:\n" << v3 << endl;
+ cout<< "\n\nNumbers added\n";
cout<< "\n" << v15 << "\n" << v20;
cout<< "\nSum:\n" << v95 << endl;
float num = 5.5650;
float num1 = -2.32;
cout<< "\n\n\nScalar addition\n";
- v3 = v2 + num;
- v6 = v10 + num1;
+ vl = v2 + num;
+ vm = v10 + num1;
cout<< "\nNumber added:" << num;
cout<< "\n\n" << v2;
- cout<< "\nResult:\n\n" << v3;
+ cout<< "\nResult:\n\n" << vl;
cout<< "\nNumber added:"<< num1;
cout<< "\n\n" << v10;
- cout<< "\nResult:\n\n"<< v6;
+ cout<< "\nResult:\n\n"<< vm;
cout<< "\n\n\nSubtraction\n";
- v6 = v10 - v12;
+ vp = v10 - v12;
cout<< "\n" << v10 << "\n" << v12;
- cout<< "\nDifference:\n" << v6 << endl;
+ cout<< "\nDifference:\n" << vp << endl;
float x = 4.0;
float x1 = -2.5;
cout<< "\n\n\nScalar subtraction\n";
v13 = v1 - x;
- v6 = v10 - x1;
+ vp = v10 - x1;
cout<< "\nNumber Subtracted:" << x;
cout<< "\n\n" << v1;
cout<< "\nResult:\n\n" << v13;
cout<< "\nNumber Subtracted:" << x1;
cout<< "\n\n" << v10;
- cout<< "\nResult:\n\n" << v6;
+ cout<< "\nResult:\n\n" << vp;
float xx = 7.2;
cout<<"\n\n\nMultiplication\n";
cout<< "\nProduct:\n" << v13 << endl;
cout<< "\n\n\nMultiplication matrix x vector:\n";
- matrix<float> m102 = m32 * v22;
+ matrix<float,3,3> m102 = m32 * v22;
cout<< "\n" << m32 <<"\n"<< v22;
cout<< "\nProduct:\n"<< m102 << endl;
cout<< "\n\n\nVector x Multiplication matrix:\n";
- vect<float> v102 = (v22 * m12);
+ vect<float,3> v102 = (v22 * m12);
cout<< "\n" << v22 << "\n" << m12;
cout<< "\nProduct:\n" << v102 << endl;
- float val = mul(v22, m32);
- cout<< "\n" << v22 << "\n" << m32;
- cout<< "\nProduct:\n" << val << endl;
float z = 3.50;
float z1 = -5.567;
cout<< "\n\n\nScalar multiplication\n";
v13 = v1 * z;
- v6 = v12 * z1;
+ v61 = v12 * z1;
cout<< "\nNumber Multiplied:" << z;
cout<< "\n\n" << v1;
cout<< "\nResult:\n\n" << v13;
float num2 = 5.5;
cout<< "\n\n\nDivision\n";
- v3 = v1 / num2;
+ v31 = v1 / num2;
cout<< "\n" << v1 << "\n" << num2;
cout<< "\nResult:\n" << v3 << endl;
cout<< "\n\n\nTranspose:";
cout << "\n\n" << v20;
cout << "\nResult:\n\n";
- matrix<float> m101 = (transpose(v20));
+ matrix<float,6,1> m101 = transpose(v20);
cout << m101;
cout << "\n\n" << m101;
cout << "\nResult:\n\n";
- v20 = (transpose(m101));
+ v20 = transpose(m101);
cout << v20;
cout << "\n\nv1:\n\n" << v1;
cout << (v10 == v10);
cout << "\n\nv12:\n\n" << v12;
- cout << "\n\nv15:\n\n" << v15;
+ cout << "\n\nv15:\n\n" << vr;
cout << "\n\n\nv12 != v15 :";
- cout << (v12 != v15);
+ cout << (v12 != vr);
cout << "\n\nv15:\n\n" << v15;
cout << "\n\nv15:\n\n" << v15;
cout << (v15 != v15);
cout<< "\n\nAssignment\n";
- v3 = v1;
+ v3 = vf;
cout<< "Input \n" << v1;
cout<< "\nOutput:\n" << v3 << endl;
-
- vect<float> v111 = cross(v21, v22);
+ vect<float,3> v111 = cross(v21, v22);
cout<< "\n\n\nCross Product:";
cout << "\n\n" << v21 << "\n\n" << v22;
cout << "\nResult:\n\n" << v111;
+ float val = dot(v21, v22);
+ cout<< "\n\n\nDot Product:";
+ cout << "\n\n" << v21 << "\n\n" << v22;
+ cout << "\nResult:\n\n" << val;
+
cout << "\n\n\nQueue insert function:";
cout << "\nInput:\n\n" << v111;
insert_end(v111, (float) 0.9191919);
cout << "\n\n\nIs Initialized:";
cout << "\nInput:\n\n" << v35;
- cout << "\nResult:\n\n" << is_initialized(v35);
+ cout << "\nResult:\n\n" << is_initialized(v35) << endl;
}
#if defined (_VECTOR_H_) && defined (_MATRIX_H_)
-template <typename TYPE>
-vect<TYPE>::vect(void)
+TYPE_SIZE vect<TYPE, SIZE>::vect(void)
{
- m_vec = NULL;
+ for(int i=0;i<SIZE;i++)
+ m_vec[i] = 0;
}
-template <typename TYPE>
-vect<TYPE>::vect(const int size)
+TYPE_SIZE vect<TYPE, SIZE>::vect(TYPE vec_data[SIZE])
{
- m_size = size;
- m_vec = new TYPE [m_size]();
-}
-
-template <typename TYPE>
-vect<TYPE>::vect(const int size, TYPE *vec_data)
-{
- m_size = size;
- m_vec = new TYPE [m_size];
- for (int j = 0; j < m_size; j++)
- m_vec[j] = *vec_data++;
+ for (int j = 0; j < SIZE; j++)
+ m_vec[j] = vec_data[j];
}
-template <typename TYPE>
-vect<TYPE>::vect(const vect<TYPE>& v)
+TYPE_SIZE vect<TYPE, SIZE>::vect(const vect<TYPE, SIZE>& v)
{
- m_size = v.m_size;
- m_vec = new TYPE [m_size];
-
- for (int q = 0; q < m_size; q++)
+ for (int q = 0; q < SIZE; q++)
m_vec[q] = v.m_vec[q];
}
-template <typename TYPE>
-vect<TYPE>::~vect()
+
+TYPE_SIZE vect<TYPE, SIZE>::~vect()
{
- if (m_vec != NULL)
- delete[] m_vec;
}
-template <typename TYPE>
-vect<TYPE> vect<TYPE>::operator =(const vect<TYPE>& v)
+TYPE_SIZE vect<TYPE, SIZE> vect<TYPE, SIZE>::operator =(const vect<TYPE, SIZE>& v)
{
if (this == &v)
{
return *this;
}
-
- if (m_vec == NULL)
- {
- m_size = v.m_size;
- m_vec = new TYPE [m_size];
- }
- else
- {
- if (m_size != v.m_size)
- {
- delete[] m_vec;
-
- m_size = v.m_size;
- m_vec = new TYPE [m_size];
- }
- }
-
-
- for (int q = 0; q < m_size; q++)
+ for (int q = 0; q < SIZE; q++)
m_vec[q] = v.m_vec[q];
-
-
- return *this;
+ return *this;
}
-template <typename TYPE>
-ostream& operator <<(ostream& dout, vect<TYPE>& v)
+T_S ostream& operator <<(ostream& dout, vect<T, S>& v)
{
- for (int j = 0; j < v.m_size; j++)
+ for (int j = 0; j < S; j++)
{
dout << v.m_vec[j] << "\t";
}
return dout;
}
-template <typename T>
-vect<T> operator +(const vect<T> v1, const vect<T> v2)
+T_S vect<T, S> operator +(const vect<T, S> v1, const vect<T, S> v2)
{
- assert(v1.m_size == v2.m_size);
+ vect<T, S> v3;
- vect<T> v3(v1.m_size);
-
- for (int j = 0; j < v1.m_size; j++)
+ for (int j = 0; j < S; j++)
v3.m_vec[j] = v1.m_vec[j] + v2.m_vec[j];
return v3;
}
-template <typename T>
-vect<T> operator +(const vect<T> v, const T val)
+T_S vect<T, S> operator +(const vect<T, S> v, const T val)
{
- vect<T> v1(v.m_size);
+ vect<T, S> v1;
- for (int j = 0; j < v.m_size; j++)
+ for (int j = 0; j < S; j++)
v1.m_vec[j] = v.m_vec[j] + val;
return v1;
}
-template <typename T>
-vect<T> operator -(const vect<T> v1, const vect<T> v2)
+T_S vect<T, S> operator -(const vect<T, S> v1, const vect<T, S> v2)
{
- assert(v1.m_size == v2.m_size);
-
- vect<T> v3(v1.m_size);
+ vect<T, S> v3;
- for (int j = 0; j < v1.m_size; j++)
+ for (int j = 0; j < S; j++)
v3.m_vec[j] = v1.m_vec[j] - v2.m_vec[j];
return v3;
}
-template <typename T>
-vect<T> operator -(const vect<T> v, const T val)
+T_S vect<T, S> operator -(const vect<T, S> v, const T val)
{
- vect<T> v1(v.m_size);
+ vect<T, S> v1;
- for (int j = 0; j < v.m_size; j++)
+ for (int j = 0; j < S; j++)
v1.m_vec[j] = v.m_vec[j] - val;
return v1;
}
-template <typename T>
-matrix<T> operator *(const matrix<T> m, const vect<T> v)
+T_S_R_C matrix<T, R, S> operator *(const matrix<T, R, C> m, const vect<T, S> v)
{
- assert(m.m_rows == v.m_size);
- assert(m.m_cols == 1);
+ assert(R == S);
+ assert(C == 1);
- matrix<T> m1(m.m_rows, v.m_size);
+ matrix<T,R,S> m1;
- for (int i = 0; i < m1.m_rows; i++)
+ for (int i = 0; i < R; i++)
{
- for (int j = 0; j < m1.m_cols; j++)
+ for (int j = 0; j < S; j++)
{
m1.m_mat[i][j] = m.m_mat[i][0] * v.m_vec[j];
}
return m1;
}
-template <typename T>
-vect<T> operator *(const vect<T> v, const matrix<T> m)
+T_S_R_C vect<T, S> operator *(const vect<T, S> v, const matrix<T, R, C> m)
{
- assert(m.m_rows == v.m_size);
- assert(m.m_cols != 1);
+ assert(R == S);
+ assert(C != 1);
+ vect<T,C> v1;
- vect<T> v1(m.m_cols);
-
- for (int j = 0; j < m.m_cols; j++)
+ for (int j = 0; j < C; j++)
{
v1.m_vec[j] = 0;
- for (int k = 0; k < m.m_rows; k++)
+ for (int k = 0; k < R; k++)
v1.m_vec[j] += v.m_vec[k] * m.m_mat[k][j];
}
return v1;
}
-template <typename T>
-vect<T> operator *(const vect<T> v, const T val)
+T_S vect<T, S> operator *(const vect<T, S> v, const T val)
{
- vect<T> v1(v.m_size);
+ vect<T, S> v1;
- for (int j = 0; j < v.m_size; j++)
+ for (int j = 0; j < S; j++)
v1.m_vec[j] = v.m_vec[j] * val;
return v1;
}
-template <typename T>
-vect<T> operator /(const vect<T> v, const T val)
+T_S vect<T, S> operator /(const vect<T, S> v, const T val)
{
- vect<T> v1(v.m_size);
+ vect<T, S> v1;
- for (int j = 0; j < v.m_size; j++)
+ for (int j = 0; j < S; j++)
v1.m_vec[j] = v.m_vec[j] / val;
return v1;
}
-template <typename T>
-bool operator ==(const vect<T> v1, const vect<T> v2)
+T_S1_S2 bool operator ==(const vect<T, S1> v1, const vect<T, S2> v2)
{
- if (v1.m_size == v2.m_size)
+ if (S1==S2)
{
- for (int i = 0; i < v1.m_size; i++)
+ for (int i = 0; i < S1; i++)
if (v1.m_vec[i] != v2.m_vec[i])
return false;
}
return true;
}
-template <typename T>
-bool operator !=(const vect<T> v1, const vect<T> v2)
+T_S1_S2 bool operator !=(const vect<T, S1> v1, const vect<T, S2> v2)
{
return (!(v1 == v2));
}
-template <typename T>
-matrix<T> transpose(const vect<T> v)
+T_S matrix<T, S, 1> transpose(const vect<T, S> v)
{
- matrix<T> m(v.m_size, 1);
+ matrix<T, S, 1> m;
- for (int i = 0; i < v.m_size; i++)
+ for (int i = 0; i < S; i++)
m.m_mat[i][0] = v.m_vec[i];
return m;
}
-template <typename T>
-vect<T> transpose(const matrix<T> m)
+T_S vect<T, S> transpose(const matrix<T, S, 1> m)
{
- vect<T> v(m.m_rows);
+ vect<T, S> v;
- for (int i = 0; i < m.m_rows; i++)
+ for (int i = 0; i < S; i++)
v.m_vec[i] = m.m_mat[i][0];
return v;
}
-template <typename T>
-T mul(const vect<T> v, const matrix<T> m)
+T_S void insert_end(vect<T, S>& v, T val)
{
- assert(m.m_rows == v.m_size);
- assert(m.m_cols == 1);
-
- T result = (T) 0;
-
- for (int k = 0; k < v.m_size; k++)
- result += v.m_vec[k] * m.m_mat[k][0];
-
- return result;
-}
-
-
-template <typename T>
-void insert_end(vect<T>& v, T val)
-{
- for (int i = 0; i < (v.m_size - 1); i++)
+ for (int i = 0; i < (S - 1); i++)
v.m_vec[i] = v.m_vec[i+1];
- v.m_vec[v.m_size-1] = val;
+ v.m_vec[S-1] = val;
}
-template <typename T>
-vect<T> cross(const vect<T> v1, const vect<T> v2)
+T_S vect<T, S> cross(const vect<T, S> v1, const vect<T, S> v2)
{
- vect<T> v3(v1.m_size);
+ vect<T, S> v3;
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]));
return v3;
}
-template <typename T>
-bool is_initialized(const vect<T> v)
+T_S T dot(const vect<T, S> v1, const vect<T, S> v2)
+{
+ return (v1.m_vec[0] * v2.m_vec[0] + v1.m_vec[1] * v2.m_vec[1] + v1.m_vec[2] * v2.m_vec[2]);
+}
+
+T_S bool is_initialized(const vect<T, S> v)
{
- vect<T> v1(v.m_size);
+ vect<T, S> v1;
bool retval;
retval = (v == v1) ? false : true;
return retval;
}
-template <typename T>
-T var(const vect<T> v)
+T_S T var(const vect<T, S> v)
{
T val = 0;
T mean, var, diff;
- for (int i = 0; i < v.m_size; i++)
+ for (int i = 0; i < S; i++)
val += v.m_vec[i];
- mean = val / v.m_size;
+ mean = val / S;
val = 0;
- for (int i = 0; i < v.m_size; i++)
+ for (int i = 0; i < S; i++)
{
diff = (v.m_vec[i] - mean);
val += diff * diff;
}
- var = val / (v.m_size - 1);
+ var = val / (S - 1);
return var;
}
#include "matrix.h"
-template <typename TYPE>
-class vect {
-public:
- int m_size;
- TYPE *m_vec;
+#define TYPE_SIZE template<typename TYPE, int SIZE>
+#define T_S template<typename T, int S>
+#define T_S_R_C template<typename T, int S, int R, int C>
+#define T_S1_S2 template<typename T, int S1, int S2>
+TYPE_SIZE class vect {
+public:
+ TYPE m_vec[SIZE];
vect(void);
- vect(const int size);
- vect(const int size, TYPE *vec_data);
- vect(const vect<TYPE>& v);
+ vect(TYPE vec_data[SIZE]);
+ vect(const vect<TYPE, SIZE>& v);
~vect();
- vect<TYPE> operator =(const vect<TYPE>& v);
+ vect<TYPE, SIZE> operator =(const vect<TYPE, SIZE>& v);
- template<typename T> friend ostream& operator << (ostream& dout,
- 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 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 vect<T> v2);
- template<typename T> friend vect<T> operator *(const vect<T> v,
- const matrix<T> m);
- template<typename T> friend vect<T> operator *(const vect<T> v,
- const T val);
- template<typename T> friend vect<T> operator /(const vect<T> v1,
- const T val);
- 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);
+ T_S friend ostream& operator << (ostream& dout, vect<T, S>& v);
+ T_S friend vect<T, S> operator +(const vect<T, S> v1, const vect<T, S> v2);
+ T_S friend vect<T, S> operator +(const vect<T, S> v, const T val);
+ T_S friend vect<T, S> operator -(const vect<T,S> v1, const vect<T,S> v2);
+ T_S friend vect<T, S> operator -(const vect<T,S> v, const T val);
+ T_S_R_C friend matrix<T, R, S> operator *(const matrix<T, R, C> v1, const vect<T, S> v2);
+ T_S_R_C friend vect<T, S> operator *(const vect<T, S> v, const matrix<T, R, C> m);
+ T_S friend vect<T, S> operator *(const vect<T, S> v, const T val);
+ T_S friend vect<T, S> operator /(const vect<T,S> v1, const T val);
+ T_S1_S2 friend bool operator ==(const vect<T, S1> v1, const vect<T, S2> v2);
+ T_S1_S2 friend bool operator !=(const vect<T, S1> v1, const vect<T, S2> v2);
- 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);
+ T_S friend void insert_end(vect<T, S>& v, T val);
+ T_S friend matrix<T, S, 1> transpose(const vect<T, S> v);
+ T_S friend vect<T, S> transpose(const matrix<T, S, 1> m);
+ T_S friend vect<T, S> cross(const vect<T, S> v1, const vect<T, S> v2);
+ T_S friend T dot(const vect<T, S> v1, const vect<T, S> v2);
+ T_S friend T var(const vect<T, S> v);
+ T_S friend bool is_initialized(const vect<T, S> v);
};
#include "vector.cpp"
#endif /* _VECTOR_H_ */
+
projects / platform / core / system / sensord.git / commitdiff