Atsumi Toda / Mbed OS UAV_Logger1

UAVの姿勢推定に使用するプログラム。

Dependencies:   MPU6050_alter

Vector/Vector_Matrix_operator.cpp@2:e6496a794bde, 2019-05-24 (annotated)

Committer:
Joeatsumi
Date:
Fri May 24 05:57:12 2019 +0000
Revision:
2:e6496a794bde
mpu6050 and GHC5883L for atitude estimation for UAV.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 2:e6496a794bde 1 #include "Vector_Matrix_operator.h"
Joeatsumi 2:e6496a794bde 2
Joeatsumi 2:e6496a794bde 3 Vector operator*(const Matrix& lhm, const Vector& rhv) {
Joeatsumi 2:e6496a794bde 4 if (lhm.GetCol() != rhv.GetDim()) error("Irregular Dimention");
Joeatsumi 2:e6496a794bde 5 Vector retVec(lhm.GetRow());
Joeatsumi 2:e6496a794bde 6
Joeatsumi 2:e6496a794bde 7 for (int i = 1; i <= lhm.GetRow(); i++) {
Joeatsumi 2:e6496a794bde 8 float temp = 0.0f;
Joeatsumi 2:e6496a794bde 9 for (int j = 1; j <= rhv.GetDim(); j++) {
Joeatsumi 2:e6496a794bde 10 temp += lhm.GetComp(i, j)*rhv.GetComp(j);
Joeatsumi 2:e6496a794bde 11 }
Joeatsumi 2:e6496a794bde 12 retVec.SetComp(i, temp);
Joeatsumi 2:e6496a794bde 13 }
Joeatsumi 2:e6496a794bde 14
Joeatsumi 2:e6496a794bde 15 retVec.CleanUp();
Joeatsumi 2:e6496a794bde 16
Joeatsumi 2:e6496a794bde 17 return retVec;
Joeatsumi 2:e6496a794bde 18 }
Joeatsumi 2:e6496a794bde 19
Joeatsumi 2:e6496a794bde 20 Vector operator*(const Vector& lhv, const Matrix& rhm) {
Joeatsumi 2:e6496a794bde 21 if (lhv.GetDim() != rhm.GetRow()) error("Irregular Dimention");
Joeatsumi 2:e6496a794bde 22 Vector retVec(rhm.GetCol());
Joeatsumi 2:e6496a794bde 23
Joeatsumi 2:e6496a794bde 24 for (int i = 1; i <= rhm.GetCol(); i++) {
Joeatsumi 2:e6496a794bde 25 float temp = 0.0f;
Joeatsumi 2:e6496a794bde 26 for (int j = 1; j <= lhv.GetDim(); j++) {
Joeatsumi 2:e6496a794bde 27 temp += lhv.GetComp(j) * rhm.GetComp(j, i);
Joeatsumi 2:e6496a794bde 28 }
Joeatsumi 2:e6496a794bde 29 retVec.SetComp(i, temp);
Joeatsumi 2:e6496a794bde 30 }
Joeatsumi 2:e6496a794bde 31
Joeatsumi 2:e6496a794bde 32 retVec.CleanUp();
Joeatsumi 2:e6496a794bde 33
Joeatsumi 2:e6496a794bde 34 return retVec;
Joeatsumi 2:e6496a794bde 35 }