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

Dependencies:   mbed

Revision:
0:6a28eb668082
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Vector/Vector.cpp	Wed Jan 30 11:39:03 2019 +0000
@@ -0,0 +1,177 @@
+#include "myConstants.h"
+#include "Vector.h"
+
+
+Vector::Vector(int dim) : dim(dim), components(0){
+    components = new float[dim];
+    if (!components) error("Memory Allocation Error");
+    for(int i=0; i<dim; i++) components[i] = 0.0f;
+}
+
+
+Vector::~Vector() {
+    delete[] components;
+}
+
+Vector::Vector(const Vector& v) : dim(v.dim), components(0) {
+    components = new float[dim];
+    if (!components) error("Memory Allocation Error");
+    memcpy(components, v.GetpComponents(), sizeof(float)*dim);
+}
+
+Vector& Vector::operator=(const Vector& v) {
+    if (this == &v) return *this;
+    dim = v.dim;
+    delete[] components;
+    components = new float[dim];
+    if (!components) error("Memory Allocation Error");
+    memcpy(components, v.GetpComponents(), sizeof(float)*dim);
+
+    return *this;
+}
+
+Vector Vector::operator+() {
+    return *this;
+}
+
+Vector Vector::operator-() {
+    Vector retVec(*this);
+    retVec *= -1;
+    return retVec;
+}
+
+Vector& Vector::operator*=(float c) {
+    for (int i = 0; i < dim; i++) {
+        components[i] *= c;
+    }
+
+    return *this;
+}
+
+Vector& Vector::operator/=(float c) {
+    if (fabs(c) < NEARLY_ZERO) error("Division by Zero");
+    for (int i = 0; i < dim; i++) {
+        components[i] /= c;
+    }
+
+    return *this;
+}
+
+Vector& Vector::operator+=(const Vector& v) {
+    if (dim != v.dim) error("failed to add: Irregular Dimention");
+    for (int i = 0; i < dim; i++) {
+        components[i] += v.components[i];
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+Vector& Vector::operator-=(const Vector& v) {
+    if (dim != v.dim) error("failed to subtract: Irregular Dimention");
+    for (int i = 0; i < dim; i++) {
+        components[i] -= v.components[i];
+    }
+
+    this->CleanUp();
+
+    return *this;
+}
+
+void Vector::SetComp(int dimNo, float val) {
+    if (dimNo > dim) error("Index Out of Bounds Error");
+    components[dimNo-1] = val;
+}
+
+void Vector::SetComps(float* pComps) {
+    memcpy(components, pComps, sizeof(float) * dim);
+}
+
+float Vector::GetNorm() const {
+    float norm = 0.0f;
+    for (int i = 0; i < dim; i++) {
+        norm += components[i] * components[i];
+    }
+    return sqrt(norm);
+}
+
+Vector Vector::Normalize() const {
+    float norm = GetNorm();
+    Vector temp(*this);
+    for (int i = 0; i < dim; i++) {
+        temp.components[i] /= norm;
+    }
+    temp.CleanUp();
+    return temp;
+}
+
+Vector Vector::GetParaCompTo(Vector v) {
+    Vector norm_v = v.Normalize();
+    return (*this * norm_v) * norm_v;
+}
+
+Vector Vector::GetPerpCompTo(Vector v) {
+    return (*this - this->GetParaCompTo(v));
+}
+
+void Vector::CleanUp() {
+    float maxComp = 0.0f;
+    for (int i = 0; i < dim; i++) {
+        if (fabs(components[i]) > maxComp) maxComp = fabs(components[i]);
+    }
+    if (maxComp > NEARLY_ZERO) {
+        for (int i = 0; i < dim; i++) {
+            if (fabs(components[i]) / maxComp < ZERO_TOLERANCE) components[i] = 0.0f;
+        }
+    }
+}
+
+Vector operator+(const Vector& lhv, const Vector& rhv) {
+    Vector retVec(lhv);
+    retVec += rhv;
+    return retVec;
+}
+
+Vector operator-(const Vector& lhv, const Vector& rhv) {
+    Vector retVec(lhv);
+    retVec -= rhv;
+    return retVec;
+}
+
+Vector Cross(const Vector& lhv, const Vector& rhv) {
+    if (lhv.GetDim() != 3) error("failed to cross: variable 'dim' must be 3");
+    if (lhv.GetDim() != rhv.GetDim()) error("failed to cross: Irregular Dimention");
+
+    Vector retVec(lhv.GetDim());
+
+    for (int i = 0; i < lhv.GetDim(); i++) {
+        retVec.SetComp(i + 1, lhv.GetComp((i + 1) % 3 + 1) * rhv.GetComp((i + 2) % 3 + 1)
+            - lhv.GetComp((i + 2) % 3 + 1) * rhv.GetComp((i + 1) % 3 + 1));
+    }
+
+    return retVec;
+}
+
+Vector operator*(const float c, const Vector& rhv) {
+    Vector retVec(rhv);
+    retVec *= c;
+    return retVec;
+}
+
+Vector operator*(const Vector& lhv, const float c) {
+    Vector retVec(lhv);
+    retVec *= c;
+    return retVec;
+}
+
+float operator*(const Vector& lhv, const Vector& rhv) {
+    if (lhv.GetDim() != rhv.GetDim()) error("Irregular Dimention");
+    float retVal = 0.0f;
+
+    for (int i = 1; i <= lhv.GetDim(); i++) {
+        retVal += lhv.GetComp(i) * rhv.GetComp(i);
+    }
+
+    return retVal;
+}