arxモデル同定のソースコード

#include <fstream>
#include <string>
#include <vector>
#include <iostream>
#include "csv_import.h"
#include "3_arx_structure_function.h"

using namespace std;

// =============================================================================================================================================
// 実行プログラム
// =============================================================================================================================================
int main()
{
    //------------------------------------------------------------------------------------------------------------------------------------------
    // インターフェース
    string analysis_file_name, validation_file_name;
    cout << "Enter analysis data file name" << endl;
    cin  >> analysis_file_name;
    cout << "Enter validation data file name" << endl;
    cin  >> validation_file_name;

    //------------------------------------------------------------------------------------------------------------------------------------------
    // csv取り込み
    vector<float> time, input, output;
    vector< vector<float> >  data = csv_import( analysis_file_name );
    
    for (int k=0; k<data[0].size(); k++ )
    {
          time.push_back( data[0][k] );
         input.push_back( data[1][k] );
        output.push_back( data[2][k] );
    }

    //------------------------------------------------------------------------------------------------------------------------------------------
    // 「0_0_condition.csv」から解析条件を読み取り
    ifstream inputfile( "./data_strage/0_0_condition.csv" );
    string   line;
    vector<string> strvec;

    getline(inputfile, line); // 1行目を捨てる

    getline(inputfile, line); // 出力次数の条件読み取り
    strvec = split(line, ',');
    int output_order_limit = stof( strvec[1] ); // 出力次数の上限

    getline(inputfile, line); // 入力次数の条件読み取り
    strvec = split(line, ',');
    int input_order_limit = stof( strvec[1] ); // 入力次数の上限

    getline(inputfile, line); // むだ時間の読み取り
    strvec = split(line, ',');
    int dead_time = stof( strvec[1] ); // 入力次数の上限

    //------------------------------------------------------------------------------------------------------------------------------------------
    // arxモデルの係数計算・行列に保存
    vector< vector< vector<float> > > arx_coefficient( output_order_limit + 1, vector<vector<float>>( input_order_limit + 1, vector<float>( output_order_limit + input_order_limit ) ) );
    arx_coefficient = arx_coefficient_matrix( output_order_limit, input_order_limit, dead_time, output, input );

    // csv出力
    ofstream outputfile1("./data_strage/3_1_arx_coefficient.csv");
    
    // 第1行目の記入
    outputfile1 << "y_order" << "," << "u_order" << "," << "dead_time" << "," << ",";
    for ( int i=0; i<output_order_limit + input_order_limit; i++ )
    {
        if ( i<output_order_limit )
        {
            outputfile1 << "a_" << i+1 << ",";
        }
        else
        {
            outputfile1 << "b_" << i- (output_order_limit - 1) << ",";
        }
    }
    outputfile1 << endl;

    // 算出した係数の記入
    for ( int output_order = 1; output_order <= output_order_limit; output_order++ )
    {
        for ( int input_order = 1; input_order <= input_order_limit; input_order++ )
        {
            outputfile1 << output_order << "," << input_order << "," << dead_time << "," << ",";

            for ( int coefficient = 0; coefficient < output_order_limit + input_order_limit; coefficient++ )
            {
                outputfile1 << arx_coefficient[output_order][input_order][coefficient] << ",";
            }
            outputfile1 << endl;
        }
    }
    cout << "arx coefficient calculation is completed" << endl;

    //------------------------------------------------------------------------------------------------------------------------------------------
    // 推定値計算
    // csv取り込み
    vector<float> time_validation, input_validation, output_validation;
    vector< vector<float> >  data_validation = csv_import( validation_file_name );

    for (int k=0; k<data_validation[0].size(); k++ )
    {
          time_validation.push_back( data_validation[0][k] );
         input_validation.push_back( data_validation[1][k] );
        output_validation.push_back( data_validation[2][k] );
    }

    // 推定値の計算・行列に保存
    vector< vector< vector<float> > > output_estimate( output_order_limit + 1, vector<vector<float>>( input_order_limit + 1, vector<float>( time_validation.size() ) ) );
    output_estimate = y_estimate_matrix( output_order_limit, input_order_limit, dead_time, output_validation, input_validation, arx_coefficient );

    // CSV出力
    ofstream outputfile2("./data_strage/3_2_estimate.csv");

    // 第1行目の記入
    outputfile2 << "time[sec]" << "," << "experiment" << ",";
    for ( int output_order=1; output_order<=output_order_limit; output_order++ )
    {
        for ( int input_order=1; input_order<=input_order_limit; input_order++ )
        {
            outputfile2 << "y" << output_order << "-u" << input_order << "estimate" << ",";
        }
    }
    outputfile2 << endl;

    // 実験データ・推定値の出力
    for ( int k=0; k<time_validation.size(); k++ )
    {
        outputfile2 << time_validation[k]  << "," << output_validation[k] << ",";
        for ( int output_order=1; output_order<=output_order_limit; output_order++ )
        {
            for ( int input_order=1; input_order<=input_order_limit; input_order++ )
            {
                outputfile2 << output_estimate[output_order][input_order][k] << ",";
            }
        }
        outputfile2 << endl;
    }
    cout << "estimate values calculation is completed" << endl;

    //------------------------------------------------------------------------------------------------------------------------------------------
    // 損失関数の計算
    vector< vector<float> > loss_function_values( 11, vector<float>(11) );

    for ( int output_order=1; output_order<=output_order_limit; output_order++ )
    {
        for ( int input_order=1; input_order<=input_order_limit; input_order++ )
        {
            loss_function_values[output_order][input_order] = loss_function_calc( output_order, input_order, dead_time, output_validation, output_estimate[output_order][input_order] );
        }
    }

    // CSV出力
    ofstream outputfile3("./data_strage/3_3_loss_function.csv");

    // 第1行目の記入
    outputfile3 << ",";
    for (  int input_order=1; input_order<=input_order_limit; input_order++)
    {
        outputfile3 << "u" << input_order << ",";
    }
    outputfile3 << endl;

    // 実験データと第1行目の記入
    for ( int output_order=1; output_order<=output_order_limit; output_order++ )
    {
        outputfile3 << "y" << output_order << ",";
        for ( int input_order=1; input_order<=input_order_limit; input_order++ )
        {
            outputfile3 << loss_function_values[output_order][input_order];
            outputfile3 << ",";
        }
        outputfile3 << endl;
    }
    cout << "loss function calculation is completed" << endl;

    //------------------------------------------------------------------------------------------------------------------------------------------
    // 損失関数が最小となる出力次数、入力次数を探す
    int   output_order_min=1, input_order_min=1;
    float loss_function_values_min = loss_function_values[1][1];
    
    for ( int output_order=1; output_order<=output_order_limit; output_order++ )
    {
        for ( int input_order=1; input_order<=input_order_limit; input_order++ )
        {
            float temp = loss_function_values[output_order][input_order];
            if ( loss_function_values_min > temp )
            {
                loss_function_values_min = temp;
                output_order_min         = output_order;
                input_order_min          = input_order;
            }
        }
    }

    //CSV出力（損失関数が最小となるarxモデルの次数と係数）
    ofstream outputfile4("./data_strage/3_4_arx_model_coefficient.csv");

    // 第1行目の記入
    outputfile4 << "y_order" << "," << "u_order" << "," << "dead_time" << "," << ",";
    for ( int i=0; i<output_order_limit + input_order_limit; i++ )
    {
        if ( i<output_order_limit )
        {
            outputfile4 << "a_" << i+1 << ",";
        }
        else
        {
            outputfile4 << "b_" << i - (output_order_limit - 1) << ",";
        }
    }
    outputfile4 << endl;

    // 損失関数が最小となるarxモデルの係数を出力
    outputfile4 << output_order_min << "," << input_order_min << "," << dead_time << "," << ",";
    for ( int coefficient=0; coefficient < output_order_limit + input_order_limit; coefficient++ )
    {
        outputfile4 << arx_coefficient[output_order_min][input_order_min][coefficient] << ",";
    }

    //CSV出力（損失関数が最小となるarxモデルの推定値）
    ofstream outputfile5("./data_strage/3_5_arx_model_estimate.csv");

    // 第1行目の記入
    outputfile5 << "time[sec]" << "," << "experiment" << "," << "estimate" << endl;
    // 推定値の出力
    for ( int k=0; k<time_validation.size(); k++ )
    {
        outputfile5 << time_validation[k] << "," << output_validation[k] << "," << output_estimate[output_order_min][input_order_min][k] << endl;
    }

    cout << "arx structure calculation is completed" << endl;
}

#include <vector>
using namespace std;

// 逆行列計算
vector< vector<float> > inv_matrix( vector< vector<float> > matrix );

// 最大値選択（2値）
float max( float input1, float input2 );

// 最小値選択（2値）
float min( float input1, float input2 );

// arxモデルの係数同定用関数（システム同定の基礎P131）---------------------------------------------------------------------------------------------------------
// ベクトルΦ(k)の導出
vector<float> vector_phi_k( int output_order, int input_order, int dead_time, vector<float> output, vector<float> input, int k );

// ベクトルfの導出
vector<float> vector_f( int output_order, int input_order, int dead_time, vector<float> output, vector<float> input );

// 行列Rの導出
vector<vector<float>> matrix_R( int output_order, int input_order, int dead_time, vector<float> output, vector<float> input );

// arxモデルの係数導出
vector<float> arx_coefficient_calc( int output_order, int input_order, int dead_time, vector<float> output, vector<float> input );

// arxモデルの係数を行列として保存する
vector< vector< vector<float> > > arx_coefficient_matrix( int output_order_limit, int input_order_limit, int dead_time, vector<float> output, vector<float> input );

// 損失関数導出関連の関数------------------------------------------------------------------------------------------------------------------------------------------
// k時点の推定値の算出
float y_k_estimate( int output_order_limit, int output_order, int input_order, int dead_time, vector<float> output, vector<float> input, vector<float> coefficient, int k );

// 出力次数・入力次数・各時間の推定値を行列として保存
vector< vector< vector<float> > > y_estimate_matrix( int output_order_limit, int input_order_limit, int dead_time, vector<float> output, vector<float> input, vector< vector< vector<float> > > coefficient);

// 損失関数の算出
float loss_function_calc(int output_order, int input_order, int dead_time, vector<float> experiment, vector<float> estimate );


// arx係数行列から，指定の出力次数・入力次数の係数ベクトルを選択
vector<float> coefficient_select( int output_order, int input_order, vector< vector<float> > coefficient_matrix );

#include <vector>
using namespace std;

// 逆行列計算 ---------------------------------------------------------------------------------------------------------------------------------
vector< vector<float> > inv_matrix( vector< vector<float> > matrix )
{
    int order = matrix[0].size();
    vector< vector<float> > result( order, vector<float>(order) );
    int i, j, k;
    float buf;

    // 単位行列をつくる
    for ( i=0; i<order; i++ )
    {
        for ( j=0; j<order; j++ )
        {
            if ( i==j )
            {
                result[i][j] = 1;
            }
            else
            {
                result[i][j] = 0;
            }
        }
    }

    // 掃き出し法
    for ( i=0; i<order; i++ )
    {
        buf = 1 / matrix[i][i];
        for ( j=0; j<order; j++ )
        {
            matrix[i][j] *= buf;
            result[i][j] *= buf;
        }
        for ( j=0; j<order; j++ )
        {
            if( i!=j )
            {
                buf = matrix[j][i];
                for( k=0; k<order; k++ )
                {
                    matrix[j][k] -= matrix[i][k]*buf;
                    result[j][k] -= result[i][k]*buf;
                }
            }
        }
    }
    return result;
}

#include <vector>
using namespace std;

// 最大値の選択（2値）--------------------------------------------------------------------------------------------------------------------------
float max( float input1, float input2 )
{
    float result=0;

    if( input1<=input2 )
    {
        result = input2;
    }
    else if( input1>input2 )
    {
        result = input1;
    }
    return result;
}

#include <vector>
using namespace std;

// 最大値の選択（2値）--------------------------------------------------------------------------------------------------------------------------
float min( float input1, float input2 )
{
    float result=0;

    if( input1>=input2 )
    {
        result = input2;
    }
    else if( input1<input2 )
    {
        result = input1;
    }
    return result;
}

#include <vector>
using namespace std;

vector<float> vector_phi_k( int output_order, int input_order, int dead_time, vector<float> output, vector<float> input, int k )
{
    int sum_order = input_order + output_order;
    int i_out = 1, i_in = 0;

    vector<float> result;
    for ( int i=0; i<sum_order; i++ )
    {
        if( i<output_order )
        {
            result.push_back( -output[ k - i_out ] );
            i_out++;
        }
        else
        {
            result.push_back( input[ k - dead_time - i_in ] );
            i_in++;
        }
    }
    return result;
}

#include <vector>
#include "3_arx_structure_function.h"

using namespace std;

// ベクトルfの作成 -----------------------------------------------------------------------------------------------------------------------------
vector<float> vector_f( int output_order, int input_order, int dead_time, vector<float> output, vector<float> input )
{
    int sum_order = input_order + output_order;
    int k_start   = max( output_order, dead_time+input_order );

    vector<float> result( sum_order );
    for ( int k=k_start; k<input.size(); k++ )
    {
        // k時点でのベクトルΦ(k)を求める
        vector<float> phi_k = vector_phi_k( output_order, input_order, dead_time, output, input, k );
        for ( int i=0; i<sum_order; i++ )
        {
            // ベクトルΦ(k)の各要素にy[k]を掛ける
            result[i] += phi_k[i] * output[k];
        }
    }

    // 各要素をデータ数で割る
    for ( int i=0; i<result.size(); i++ )
    {
        result[i] = result[i] / ( input.size() - k_start);
    }
    return result;
}

#include <vector>
#include "3_arx_structure_function.h"

using namespace std;

// 行列Rの作成 ---------------------------------------------------------------------------------------------------------------------------------
vector<vector<float>> matrix_R( int output_order, int input_order, int dead_time, vector<float> output, vector<float> input )
{
    int sum_order = input_order + output_order;
    int k_start   = max(output_order, dead_time+input_order);
    
    vector< vector<float> > result( sum_order, vector<float>( sum_order ) );
    for ( int k=k_start; k<output.size(); k++ )
    {
        // k時点でのベクトルΦ(k)
        vector<float> phi_k = vector_phi_k( output_order, input_order, dead_time, output, input, k );
        
        // Φ(k)とΦ(k)^Tの積
        for( int row=0; row<sum_order; row++ )
        {
            for ( int col=0; col<sum_order; col++ )
            {
                result[row][col] += phi_k[row]*phi_k[col];
            }
        }
    }

    // 行列の各要素をデータ数で割る
    for( int row=0; row<sum_order; row++ )
    {
        for ( int col=0; col<sum_order; col++ )
        {
            result[row][col] = result[row][col] / ( input.size() - k_start );
        }
    }
    return result;
}

#include <vector>
#include "3_arx_structure_function.h"

using namespace std;

// 同定モデルの係数導出 --------------------------------------------------------------------------------------------------------------------------
vector<float> arx_coefficient_calc( int output_order, int input_order, int dead_time, vector<float> output, vector<float> input )
{
    int sum_order = input_order + output_order;
    vector<float> result( sum_order );

    // ベクトルfを作る
    vector<float> f = vector_f( output_order, input_order, dead_time, output, input );

    // 行列Rを作る
    vector< vector<float> > R = matrix_R( output_order, input_order, dead_time, output, input );
    
    // 行列Rの逆行列
    vector< vector<float> > inv_R = inv_matrix( R );

    // result = inv_R × f
    for ( int i=0; i<sum_order; i++ )
    {
        for ( int j=0; j<sum_order; j++ )
        {
            result[i] += inv_R[i][j] * f[j];
        }
    }
    return result;
}

#include <vector>
#include "3_arx_structure_function.h"

using namespace std;

// arxモデルの係数を行列として保存する
vector< vector< vector<float> > > arx_coefficient_matrix( int output_order_limit, int input_order_limit, int dead_time, vector<float> output, vector<float> input )
{
    vector< vector< vector<float> > > result( output_order_limit + 1, vector<vector<float>>( input_order_limit + 1, vector<float>( output_order_limit + input_order_limit ) ) );
    vector<float> arx_coefficient_buf( output_order_limit + input_order_limit );
    
     for ( int output_order=1; output_order<=output_order_limit; output_order++ )
    {
        for ( int input_order=1; input_order<=input_order_limit; input_order++ )
        {
            int element = 0;
            vector<float> arx_coefficient_buf = arx_coefficient_calc( output_order, input_order, dead_time, output, input );

            for ( int coefficient=0; coefficient<output_order_limit + input_order_limit; coefficient++ )
            {
                if ( coefficient<output_order )
                {
                    result[output_order][input_order][coefficient] = arx_coefficient_buf[element];
                    element ++;
                }
                else if( ( coefficient>=output_order_limit ) && ( coefficient<output_order_limit+input_order ) )
                {
                    result[output_order][input_order][coefficient] = arx_coefficient_buf[element];
                    element ++;
                }
                else
                {
                    result[output_order][input_order][coefficient] = 0;
                }
            }
        }
    }
    
    return result;
}

#include <vector>
#include "3_arx_structure_function.h"

using namespace std;

// 推定値の算出 ---------------------------------------------------------------------------------------------------------------------------------
float y_k_estimate( int output_order_limit, int output_order, int input_order, int dead_time, vector<float> output, vector<float> input, vector<float> coefficient, int k )
{
    float result = 0;

    if ( k==0 )
    {
        result = output[0];
    }
    else
    {
        for ( int i=0; i<min(output_order,k); i++ )
        {
            result += -coefficient[i] * output[ k-i-1 ];
        }
        for ( int i=0; i<min(input_order,k); i++ )
        {
            result += coefficient[i+output_order_limit] * input[ k-dead_time-i ];
        }
    }
    return result;
}

#include <vector>
#include "3_arx_structure_function.h"

using namespace std;

// 推定値を行列として保存
vector< vector< vector<float> > > y_estimate_matrix( int output_order_limit, int input_order_limit, int dead_time, vector<float> output, vector<float> input, vector< vector< vector<float> > > arx_coefficient)
{
    vector< vector< vector<float> > > result( output_order_limit + 1, vector<vector<float>>( input_order_limit + 1, vector<float>( output.size() ) ) );

    for ( int output_order=1; output_order<=output_order_limit; output_order++ )
    {
        for ( int input_order=1; input_order<=input_order_limit; input_order++ )
        {
            for ( int k=0; k<output.size(); k++ )
            {
                float tmp = y_k_estimate( output_order_limit, output_order, input_order, dead_time, output, input, arx_coefficient[output_order][input_order], k );
                result[output_order][input_order][k] = tmp;
            }
        }
    }
    return result;
}

#include <vector>
#include <bits/stdc++.h>
#include "3_arx_structure_function.h"

using namespace std;

// 損失関数
float loss_function_calc(int output_order, int input_order, int dead_time, vector<float> experiment, vector<float> estimate )
{
    float result = 0;
    int   k_start = max(output_order, input_order+dead_time);

    for ( int k=k_start; k<experiment.size(); k++ )
    {
        result += pow( experiment[k] - estimate[k], 2 );
    }
    result = result / (experiment.size() - k_start);
    return result;
}

#include <vector>
using namespace std;

// arx係数行列から，指定の出力次数・入力次数の係数ベクトルを選択-------------------------------------------------------------------------------------
vector<float> coefficient_select( int output_order, int input_order, vector< vector<float> > coefficient_matrix )
{
    vector<float> result;
    int row = 10*(output_order-1) + (input_order-1);

    for ( int col=0; col<20; col++ )
    {
        result.push_back( coefficient_matrix[row][col] );
    }
    return result;
}