Atsumi Toda / Mbed 2 deprecated Matrix_calculation_test

慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 （行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します）

Dependencies:   mbed

Vector/Vector.cpp@0:6a28eb668082, 2019-01-30 (annotated)

Committer:
Joeatsumi
Date:
Wed Jan 30 11:39:03 2019 +0000
Revision:
0:6a28eb668082
Direction cosine matrix and it's calculation.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:6a28eb668082 1 #include "myConstants.h"
Joeatsumi 0:6a28eb668082 2 #include "Vector.h"
Joeatsumi 0:6a28eb668082 3
Joeatsumi 0:6a28eb668082 4
Joeatsumi 0:6a28eb668082 5 Vector::Vector(int dim) : dim(dim), components(0){
Joeatsumi 0:6a28eb668082 6 components = new float[dim];
Joeatsumi 0:6a28eb668082 7 if (!components) error("Memory Allocation Error");
Joeatsumi 0:6a28eb668082 8 for(int i=0; i<dim; i++) components[i] = 0.0f;
Joeatsumi 0:6a28eb668082 9 }
Joeatsumi 0:6a28eb668082 10
Joeatsumi 0:6a28eb668082 11
Joeatsumi 0:6a28eb668082 12 Vector::~Vector() {
Joeatsumi 0:6a28eb668082 13 delete[] components;
Joeatsumi 0:6a28eb668082 14 }
Joeatsumi 0:6a28eb668082 15
Joeatsumi 0:6a28eb668082 16 Vector::Vector(const Vector& v) : dim(v.dim), components(0) {
Joeatsumi 0:6a28eb668082 17 components = new float[dim];
Joeatsumi 0:6a28eb668082 18 if (!components) error("Memory Allocation Error");
Joeatsumi 0:6a28eb668082 19 memcpy(components, v.GetpComponents(), sizeof(float)*dim);
Joeatsumi 0:6a28eb668082 20 }
Joeatsumi 0:6a28eb668082 21
Joeatsumi 0:6a28eb668082 22 Vector& Vector::operator=(const Vector& v) {
Joeatsumi 0:6a28eb668082 23 if (this == &v) return *this;
Joeatsumi 0:6a28eb668082 24 dim = v.dim;
Joeatsumi 0:6a28eb668082 25 delete[] components;
Joeatsumi 0:6a28eb668082 26 components = new float[dim];
Joeatsumi 0:6a28eb668082 27 if (!components) error("Memory Allocation Error");
Joeatsumi 0:6a28eb668082 28 memcpy(components, v.GetpComponents(), sizeof(float)*dim);
Joeatsumi 0:6a28eb668082 29
Joeatsumi 0:6a28eb668082 30 return *this;
Joeatsumi 0:6a28eb668082 31 }
Joeatsumi 0:6a28eb668082 32
Joeatsumi 0:6a28eb668082 33 Vector Vector::operator+() {
Joeatsumi 0:6a28eb668082 34 return *this;
Joeatsumi 0:6a28eb668082 35 }
Joeatsumi 0:6a28eb668082 36
Joeatsumi 0:6a28eb668082 37 Vector Vector::operator-() {
Joeatsumi 0:6a28eb668082 38 Vector retVec(*this);
Joeatsumi 0:6a28eb668082 39 retVec *= -1;
Joeatsumi 0:6a28eb668082 40 return retVec;
Joeatsumi 0:6a28eb668082 41 }
Joeatsumi 0:6a28eb668082 42
Joeatsumi 0:6a28eb668082 43 Vector& Vector::operator*=(float c) {
Joeatsumi 0:6a28eb668082 44 for (int i = 0; i < dim; i++) {
Joeatsumi 0:6a28eb668082 45 components[i] *= c;
Joeatsumi 0:6a28eb668082 46 }
Joeatsumi 0:6a28eb668082 47
Joeatsumi 0:6a28eb668082 48 return *this;
Joeatsumi 0:6a28eb668082 49 }
Joeatsumi 0:6a28eb668082 50
Joeatsumi 0:6a28eb668082 51 Vector& Vector::operator/=(float c) {
Joeatsumi 0:6a28eb668082 52 if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
Joeatsumi 0:6a28eb668082 53 for (int i = 0; i < dim; i++) {
Joeatsumi 0:6a28eb668082 54 components[i] /= c;
Joeatsumi 0:6a28eb668082 55 }
Joeatsumi 0:6a28eb668082 56
Joeatsumi 0:6a28eb668082 57 return *this;
Joeatsumi 0:6a28eb668082 58 }
Joeatsumi 0:6a28eb668082 59
Joeatsumi 0:6a28eb668082 60 Vector& Vector::operator+=(const Vector& v) {
Joeatsumi 0:6a28eb668082 61 if (dim != v.dim) error("failed to add: Irregular Dimention");
Joeatsumi 0:6a28eb668082 62 for (int i = 0; i < dim; i++) {
Joeatsumi 0:6a28eb668082 63 components[i] += v.components[i];
Joeatsumi 0:6a28eb668082 64 }
Joeatsumi 0:6a28eb668082 65
Joeatsumi 0:6a28eb668082 66 this->CleanUp();
Joeatsumi 0:6a28eb668082 67
Joeatsumi 0:6a28eb668082 68 return *this;
Joeatsumi 0:6a28eb668082 69 }
Joeatsumi 0:6a28eb668082 70
Joeatsumi 0:6a28eb668082 71 Vector& Vector::operator-=(const Vector& v) {
Joeatsumi 0:6a28eb668082 72 if (dim != v.dim) error("failed to subtract: Irregular Dimention");
Joeatsumi 0:6a28eb668082 73 for (int i = 0; i < dim; i++) {
Joeatsumi 0:6a28eb668082 74 components[i] -= v.components[i];
Joeatsumi 0:6a28eb668082 75 }
Joeatsumi 0:6a28eb668082 76
Joeatsumi 0:6a28eb668082 77 this->CleanUp();
Joeatsumi 0:6a28eb668082 78
Joeatsumi 0:6a28eb668082 79 return *this;
Joeatsumi 0:6a28eb668082 80 }
Joeatsumi 0:6a28eb668082 81
Joeatsumi 0:6a28eb668082 82 void Vector::SetComp(int dimNo, float val) {
Joeatsumi 0:6a28eb668082 83 if (dimNo > dim) error("Index Out of Bounds Error");
Joeatsumi 0:6a28eb668082 84 components[dimNo-1] = val;
Joeatsumi 0:6a28eb668082 85 }
Joeatsumi 0:6a28eb668082 86
Joeatsumi 0:6a28eb668082 87 void Vector::SetComps(float* pComps) {
Joeatsumi 0:6a28eb668082 88 memcpy(components, pComps, sizeof(float) * dim);
Joeatsumi 0:6a28eb668082 89 }
Joeatsumi 0:6a28eb668082 90
Joeatsumi 0:6a28eb668082 91 float Vector::GetNorm() const {
Joeatsumi 0:6a28eb668082 92 float norm = 0.0f;
Joeatsumi 0:6a28eb668082 93 for (int i = 0; i < dim; i++) {
Joeatsumi 0:6a28eb668082 94 norm += components[i] * components[i];
Joeatsumi 0:6a28eb668082 95 }
Joeatsumi 0:6a28eb668082 96 return sqrt(norm);
Joeatsumi 0:6a28eb668082 97 }
Joeatsumi 0:6a28eb668082 98
Joeatsumi 0:6a28eb668082 99 Vector Vector::Normalize() const {
Joeatsumi 0:6a28eb668082 100 float norm = GetNorm();
Joeatsumi 0:6a28eb668082 101 Vector temp(*this);
Joeatsumi 0:6a28eb668082 102 for (int i = 0; i < dim; i++) {
Joeatsumi 0:6a28eb668082 103 temp.components[i] /= norm;
Joeatsumi 0:6a28eb668082 104 }
Joeatsumi 0:6a28eb668082 105 temp.CleanUp();
Joeatsumi 0:6a28eb668082 106 return temp;
Joeatsumi 0:6a28eb668082 107 }
Joeatsumi 0:6a28eb668082 108
Joeatsumi 0:6a28eb668082 109 Vector Vector::GetParaCompTo(Vector v) {
Joeatsumi 0:6a28eb668082 110 Vector norm_v = v.Normalize();
Joeatsumi 0:6a28eb668082 111 return (*this * norm_v) * norm_v;
Joeatsumi 0:6a28eb668082 112 }
Joeatsumi 0:6a28eb668082 113
Joeatsumi 0:6a28eb668082 114 Vector Vector::GetPerpCompTo(Vector v) {
Joeatsumi 0:6a28eb668082 115 return (*this - this->GetParaCompTo(v));
Joeatsumi 0:6a28eb668082 116 }
Joeatsumi 0:6a28eb668082 117
Joeatsumi 0:6a28eb668082 118 void Vector::CleanUp() {
Joeatsumi 0:6a28eb668082 119 float maxComp = 0.0f;
Joeatsumi 0:6a28eb668082 120 for (int i = 0; i < dim; i++) {
Joeatsumi 0:6a28eb668082 121 if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]);
Joeatsumi 0:6a28eb668082 122 }
Joeatsumi 0:6a28eb668082 123 if (maxComp > NEARLY_ZERO) {
Joeatsumi 0:6a28eb668082 124 for (int i = 0; i < dim; i++) {
Joeatsumi 0:6a28eb668082 125 if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
Joeatsumi 0:6a28eb668082 126 }
Joeatsumi 0:6a28eb668082 127 }
Joeatsumi 0:6a28eb668082 128 }
Joeatsumi 0:6a28eb668082 129
Joeatsumi 0:6a28eb668082 130 Vector operator+(const Vector& lhv, const Vector& rhv) {
Joeatsumi 0:6a28eb668082 131 Vector retVec(lhv);
Joeatsumi 0:6a28eb668082 132 retVec += rhv;
Joeatsumi 0:6a28eb668082 133 return retVec;
Joeatsumi 0:6a28eb668082 134 }
Joeatsumi 0:6a28eb668082 135
Joeatsumi 0:6a28eb668082 136 Vector operator-(const Vector& lhv, const Vector& rhv) {
Joeatsumi 0:6a28eb668082 137 Vector retVec(lhv);
Joeatsumi 0:6a28eb668082 138 retVec -= rhv;
Joeatsumi 0:6a28eb668082 139 return retVec;
Joeatsumi 0:6a28eb668082 140 }
Joeatsumi 0:6a28eb668082 141
Joeatsumi 0:6a28eb668082 142 Vector Cross(const Vector& lhv, const Vector& rhv) {
Joeatsumi 0:6a28eb668082 143 if (lhv.GetDim() != 3) error("failed to cross: variable 'dim' must be 3");
Joeatsumi 0:6a28eb668082 144 if (lhv.GetDim() != rhv.GetDim()) error("failed to cross: Irregular Dimention");
Joeatsumi 0:6a28eb668082 145
Joeatsumi 0:6a28eb668082 146 Vector retVec(lhv.GetDim());
Joeatsumi 0:6a28eb668082 147
Joeatsumi 0:6a28eb668082 148 for (int i = 0; i < lhv.GetDim(); i++) {
Joeatsumi 0:6a28eb668082 149 retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1)
Joeatsumi 0:6a28eb668082 150 - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1));
Joeatsumi 0:6a28eb668082 151 }
Joeatsumi 0:6a28eb668082 152
Joeatsumi 0:6a28eb668082 153 return retVec;
Joeatsumi 0:6a28eb668082 154 }
Joeatsumi 0:6a28eb668082 155
Joeatsumi 0:6a28eb668082 156 Vector operator*(const float c, const Vector& rhv) {
Joeatsumi 0:6a28eb668082 157 Vector retVec(rhv);
Joeatsumi 0:6a28eb668082 158 retVec *= c;
Joeatsumi 0:6a28eb668082 159 return retVec;
Joeatsumi 0:6a28eb668082 160 }
Joeatsumi 0:6a28eb668082 161
Joeatsumi 0:6a28eb668082 162 Vector operator*(const Vector& lhv, const float c) {
Joeatsumi 0:6a28eb668082 163 Vector retVec(lhv);
Joeatsumi 0:6a28eb668082 164 retVec *= c;
Joeatsumi 0:6a28eb668082 165 return retVec;
Joeatsumi 0:6a28eb668082 166 }
Joeatsumi 0:6a28eb668082 167
Joeatsumi 0:6a28eb668082 168 float operator*(const Vector& lhv, const Vector& rhv) {
Joeatsumi 0:6a28eb668082 169 if (lhv.GetDim() != rhv.GetDim()) error("Irregular Dimention");
Joeatsumi 0:6a28eb668082 170 float retVal = 0.0f;
Joeatsumi 0:6a28eb668082 171
Joeatsumi 0:6a28eb668082 172 for (int i = 1; i <= lhv.GetDim(); i++) {
Joeatsumi 0:6a28eb668082 173 retVal += lhv.GetComp(i) * rhv.GetComp(i);
Joeatsumi 0:6a28eb668082 174 }
Joeatsumi 0:6a28eb668082 175
Joeatsumi 0:6a28eb668082 176 return retVal;
Joeatsumi 0:6a28eb668082 177 }