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

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Matrix.h Source File

Matrix.h

00001 #pragma once
00002 #include "mbed.h"
00003 
00004 class Matrix
00005 {
00006 public:
00007     /********** コンストラクタ デストラクタ **********/
00008     Matrix(int row, int col);
00009     Matrix(int row, int col, float* comps);
00010     ~Matrix();
00011     Matrix(const Matrix& m);
00012 
00013     /********** メンバ演算子 **********/
00014     Matrix operator-() const;
00015     Matrix& operator=(const Matrix& m);
00016     Matrix& operator+=(const Matrix& m);
00017     Matrix& operator-=(const Matrix& m);
00018     //Matrix& operator*=(const Matrix& m);
00019     Matrix& operator*=(float c);
00020     Matrix& operator/=(float c);
00021 
00022     /********** その他関数 **********/
00023     /*
00024         行列の成分を設定
00025         引数:rowNo 行番号
00026               colNo 列番号
00027               val 設定値
00028     */
00029     void SetComp(int rowNo, int colNo, float val);
00030 
00031     /*
00032         行列の成分を全て設定。全成分を一度に指定する必要がある。
00033         引数:pComps 設定値の入ったfloat配列。
00034     */
00035     void SetComps(float* pComps);
00036     /*
00037         行列式を計算する。行列が正方行列で無い場合にはエラー。
00038     */
00039     float Determinant() const;
00040     
00041     /*
00042         行列式を計算する。行列が正方行列で無い場合にはエラー。
00043     */
00044     float det() const;
00045 
00046     /*
00047         行列をLU分解する
00048         引数:sign (省略可)置換操作の符号を格納するポインタ
00049              p (省略可)置換行列を格納する行列のポインタ。(分解する行列と同じ列数の正方行列)
00050         返り値:LU分解後の行列。下三角要素がL、対角・上三角要素がUに対応する。
00051     */
00052     Matrix LU_Decompose(int* sign = 0, Matrix * p = 0) const;
00053     
00054     /*
00055         逆行列を生成する
00056         返り値で逆行列が存在するか否かを判断
00057         引数:逆行列を格納する行列
00058         返り値:逆行列が存在するか否か
00059     */
00060     float Inverse(Matrix& invm) const;
00061     
00062     /*
00063         転置行列を生成する
00064         返り値:転置行列
00065     */
00066     Matrix Transpose() const;
00067     
00068     /*
00069         行列の行の入れ替えを行う
00070         引数:rowNo1 行番号1
00071              rowNo2 行番号2
00072     */
00073     void SwapRow(int rowNo1, int rowNo2);
00074     /*
00075         行列の列の入れ替えを行う
00076         引数:colNo1 列番号1
00077              colNo2 列番号2
00078     */
00079     void SwapCol(int colNo1, int colNo2);
00080     
00081     /********** インライン関数 **********/
00082     inline int GetRow() const {
00083         return row;
00084     }
00085 
00086     inline int GetCol() const {
00087         return col;
00088     }
00089 
00090     inline const float* GetpComponents() const {
00091         return (const float*)components;
00092     }
00093 
00094     inline float GetComp(int rowNo, int colNo) const {
00095         if (rowNo > row || colNo > col) error("Index Out of Bounds Error !!");
00096         return components[(rowNo-1)*col + (colNo-1)];
00097     }
00098 
00099 private:
00100     int row;
00101     int col;
00102     float* components;
00103 
00104     /*
00105         行列の成分の中で無視できるほど小さい値を0と置き換える(掃除する)
00106     */
00107     void CleanUp();
00108 
00109 };
00110 
00111 // グローバル演算子
00112 Matrix operator+(const Matrix& lhm, const Matrix& rhm);
00113 Matrix operator-(const Matrix& lhm, const Matrix& rhm);
00114 Matrix operator*(const Matrix& lhm, const Matrix& rhm);
00115