自动驾驶-动力学模型-状态空间法A矩阵----c语言实现【附Github源码链接】

在实现最优控制的过程中，发现状态空见表达式还需要离散化处理，因此为了使最优控制LQR算法尽快落地，又写了个针对于汽车动力学模型的状态空间表达式的离散化处理程序，也是用C语言实现的。
其中公式如下：

按照上述公式，写了如下的代码，完整代码请参见我的github仓库https://github.com/JackJu-HIT/MatrixAandMatrixBDiscretization

#include 
#include 

#define SIZE  4  
#define T     0.02 
int main(){

       double MATRIX_A[SIZE][SIZE]                = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};
       double MATRIX_B[SIZE]                      = {1,2,3,4};
       double MATRIX_I[SIZE][SIZE]                = {1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};
       double MATRIX_I_Multi_A[SIZE][SIZE]        = {0};
       double MATRIX_I_Sub_A[SIZE][SIZE]          = {0};
       double MATRIX_I_Sub_A_Inverse[SIZE][SIZE]  = {0};
       double MATRIX_A_Discretization[SIZE][SIZE] = {0};
       double MATRIX_B_Discretization[SIZE]       = {0};


       // B_D = T * B
       for(int i = 0;i < SIZE;i++){
           MATRIX_B_Discretization[i] = T * MATRIX_B[i];
       }

       
       
       //MATRIX_I_Multi_A
       for(int i = 0;i < SIZE;i++){
           for(int j = 0;j < SIZE;j++){
               MATRIX_I_Multi_A[i][j] = MATRIX_I[i][j] + 0.5 * MATRIX_A[i][j] * T;
           } 
       }

       //MATRIX_I_Sub_A
        for(int i = 0;i < SIZE;i++){
           for(int j = 0;j < SIZE;j++){
               MATRIX_I_Sub_A[i][j] = MATRIX_I[i][j] - 0.5 * MATRIX_A[i][j] * T;
           } 
        }

        
        double Q[SIZE][SIZE]              = {0};
        double R[SIZE][SIZE]              = {0};
        double v[SIZE]                    = {0};
        double QT[SIZE][SIZE]             = {0};
        double RInverse[SIZE][SIZE]       = {0};
        double m_in[SIZE][SIZE]           = {0};
        double m_inverse_[SIZE][SIZE]     = {0};
        double m_inverse[SIZE][SIZE]      = {0}; 
        int    length                     = SIZE;  
    
        
        for(int k = 0;k < length;k++){

            
            if(k >= 1){
                for(int i = 0;i < k;i++){
                    for(int j = 0;j < length;j++){
                        R[i][k] += Q[j][i]*MATRIX_I_Sub_A[j][k];
                    }
                }
            }

            
            for(int j = 0;j
                if(k < 1){
                v[j] = MATRIX_I_Sub_A[j][0];
                } else {
                        v[j] = MATRIX_I_Sub_A[j][k];
                        for(int g = 0;g < k;g++){
                            v[j] -= R[g][k]*Q[j][g];
                        }
                }
            }

            
            for(int i = 0;i < length;i++){
                R[k][k] += v[i]*v[i];
            }

            R[k][k] = sqrt(R[k][k]);

            
            for(int i = 0;i < length;i++){
                Q[i][k] = v[i]/R[k][k];
            }  
        }
    
        //求解Q的转置
        for(int i = 0;i < length;i++){
        for(int j = 0;j
            QT[j][i] = Q[i][j];
        }
        }

        

        //转置
        for(int i = 0;i < length;i++){
            for(int j = 0;j < length;j++){
                m_in[j][i] = R[i][j];
            }
        }
        
    
        for(int j = 0;j < length;j++){
            //求解对角线
            m_inverse_[j][j] = 1/m_in[j][j];
            for(int i = j+1;i < length;i++){
                    double temp = 0;

                    for(int k = j;k <= i-1;k++){
                        temp += m_in[i][k]*m_inverse_[k][j];
                    }

                    m_inverse_[i][j] = -temp/m_in[i][i];
            }
        }
    
        //tranpose
        for(int i = 0;i < length;i++){
            for(int j = 0;j < length;j++){
                m_inverse[j][i] = m_inverse_[i][j];  //逆矩阵
            }
        }
    
        

        for(int i = 0;i < length;i++){
            for(int j = 0;j < length;j++){
                for(int k = 0;k < length;k++){
                    MATRIX_I_Sub_A_Inverse[i][j] += m_inverse[i][k]*QT[k][j]; 
                }
            }
        }
    
        // A_D = (I+0.5*A*T)*(I-0.5*A*T)^(-1)
        for(int i = 0;i < SIZE;i++){
            for(int j = 0;j < SIZE;j++){
                for(int k = 0;k < SIZE;k++){
                    MATRIX_A_Discretization[i][j] += MATRIX_I_Multi_A[i][k] * MATRIX_I_Sub_A_Inverse[k][j];
                }
            }
        }
    
        //打印输出结果
        printf("MATRIX_A_Discretization：n");
        for(int i = 0;i < SIZE;++i){
            for(int j = 0;j < SIZE;++j)
            {
                printf("%2.4f    ",MATRIX_A_Discretization[i][j]);
            }
            printf("n");
        }

        printf("MATRIX_B_Discretization：n");
        for(int i = 0;i < SIZE;++i){
            printf("%2.4f    ",MATRIX_B_Discretization[i]);
            printf("n");
        }
}

Linux下运行效果：