高斯消去法解線性方程組
最近在校正電子羅盤,需要解線性方程組,在 MATLAB 上求反矩陣很容易,但要在單晶片上實現就變得麻煩了,所以就寫了個高斯消去法,來求解線性方程組的解。
高斯消去法過程很直覺,先將左下三角消成 0,在將中間對角線消成 1,最後將右上三角消成 0,此時方程組之解就在矩陣最右邊,詳細說明可以參考高斯消去法 | 線代啟示錄。
目前沒有加入 nonsingular matrix 的判斷,當方程組有多個解或無解時,此程式會有問題,所以正確的方法應該要在運算前判斷矩陣是否非奇異。
高斯消去法過程很直覺,先將左下三角消成 0,在將中間對角線消成 1,最後將右上三角消成 0,此時方程組之解就在矩陣最右邊,詳細說明可以參考高斯消去法 | 線代啟示錄。
目前沒有加入 nonsingular matrix 的判斷,當方程組有多個解或無解時,此程式會有問題,所以正確的方法應該要在運算前判斷矩陣是否非奇異。
int8_t GaussianElimination( uint8_t col, uint8_t row, float *arr )
{
float tmp;
int8_t i = 0, j = 0, k = 0;
/* check nonsingular */
// if matrix is nonsingular
// return ERROR;
/* left-down to zero */
for(i = 0; i < col; i++) {
for(j = i + 1; j < row; j++) {
for(k = col; k > i - 1; k--) {
arr[j*row+k] = arr[j*row+k] - arr[j*row+i] / arr[i*row+i] * arr[i*row+k];
}
}
}
/* diagonal to one */
for(i = 0; i < col; i++) {
tmp = arr[i*row+i];
for(j = i; j < row; j++) {
arr[i*row+j] = arr[i*row+j] / tmp;
}
}
/* right-up to zero */
for(j = row - 2; j > 0; j--) {
for(i = 0; i < j; i++) {
arr[i*row+row-1] = arr[i*row+row-1] - arr[i*row+j] * arr[j*row+row-1];
}
}
return SUCCESS;
}
/* 主程式 */
#define col_n 4
#define row_n 5
float arr[col_n][row_n] = {
{1.0f, 3.0f, -5.0f, 2.0f, 94.0f}, // 1*x1 + 3*x2 - 5*x3 + 2*x4 = 94
{4.0f, -3.0f, 1.0f, 5.0f, 58.0f}, // 4*x1 - 3*x2 + 1*x3 + 5*x4 = 58
{6.0f, -2.0f, 2.0f, 4.0f, 72.0f}, // 6*x1 - 2*x2 + 2*x3 + 4*x4 = 72
{0.0f, 2.0f, 3.0f, -7.0f, -69.0f} // 0*x1 + 2*x2 + 3*x3 - 7*x4 = -69
};
int main()
{
uint8_t i = 0;
float *ptr = arr;
GaussianElimination(col_n, row_n, arr);
for(i = 0; i < col_n; i++) {
printf("%.1f\n", ptr[i*row_n+(row_n-1)]);
}
}