慣性航法で用いられる座標変換をプログラムにしました。ECI座標の初期位置を設定した後、ECI,ECEF,NED,機体座標系の変換を行います。行列計算の方法や値の設定などは、ヘッダーファイル内の記述を見れば分かると思います。 また計算結果はTeratermで確認する事が出来ます。 (行列を見る場合はtoString関数、ベクトルを見る場合はtoString_V関数を使用します)
Vector.h
00001 #pragma once 00002 #include "mbed.h" 00003 00004 class Matrix; 00005 00006 class Vector { 00007 public: 00008 Vector(int dim); 00009 ~Vector(); 00010 Vector(const Vector& v); 00011 00012 Vector& operator=(const Vector& v); 00013 Vector operator+(); 00014 Vector operator-(); 00015 Vector& operator*=(float c); 00016 Vector& operator/=(float c); 00017 Vector& operator+=(const Vector& v); 00018 Vector& operator-=(const Vector& v); 00019 00020 void SetComp(int dimNo, float val); 00021 void SetComps(float* vals); 00022 float GetNorm() const; 00023 Vector Normalize() const; 00024 Vector GetParaCompTo(Vector v); 00025 Vector GetPerpCompTo(Vector v); 00026 00027 inline int GetDim() const { 00028 return dim; 00029 } 00030 00031 inline const float* GetpComponents() const { 00032 return (const float*)components; 00033 } 00034 00035 inline float GetComp(int dimNo) const { 00036 if (dimNo > dim) error("Index Out of Bounds Error !!"); 00037 return components[dimNo-1]; 00038 } 00039 00040 void CleanUp(); 00041 00042 private: 00043 int dim; 00044 float* components; 00045 00046 Vector& operator*=(const Matrix& m); 00047 Vector& operator*=(const Vector& m); 00048 }; 00049 00050 Vector operator+(const Vector& lhv, const Vector& rhv); 00051 Vector operator-(const Vector& lhv, const Vector& rhv); 00052 Vector Cross(const Vector& lhv, const Vector& rhv); 00053 Vector operator*(const float c, const Vector& rhv); 00054 Vector operator*(const Vector& lhv, const float c); 00055 float operator*(const Vector& lhv, const Vector& rhv); 00056
Generated on Wed Jul 20 2022 18:18:03 by 1.7.2