안녕하세요 예스스탁입니다. 죄송하지만 해당 내용은 변경해 드릴수 없습니다. 사용해본 경험이 없는 언어라 수식 내용 파악이 되지 않습니다. 도움을 드리지 못해 죄송합니다. 즐거운 하루되세요 > 영원한자유 님이 쓴 글입니다. > 제목 : kalman filter 지표를 예스트렝이더로 구현 가능한가요? > static void matrix_multiply(float* A, float* B, int m, int p, int n, float* C) // Matrix Multiplication Routine { // A = input matrix (m x p) // B = input matrix (p x n) // m = number of rows in A // p = number of columns in A = number of rows in B // n = number of columns in B // C = output matrix = A*B (m x n) int i, j, k; for (i=0;i<m;i++) for(j=0;j<n;j++) { C[n*i+j]=0; for (k=0;k<p;k++) C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j]; } } static void matrix_addition(float* A, float* B, int m, int n, float* C) // Matrix Addition Routine { // A = input matrix (m x n) // B = input matrix (m x n) // m = number of rows in A = number of rows in B // n = number of columns in A = number of columns in B // C = output matrix = A+B (m x n) int i, j; for (i=0;i<m;i++) for(j=0;j<n;j++) C[n*i+j]=A[n*i+j]+B[n*i+j]; } static void matrix_subtraction(float* A, float* B, int m, int n, float* C) // Matrix Subtraction Routine { // A = input matrix (m x n) // B = input matrix (m x n) // m = number of rows in A = number of rows in B // n = number of columns in A = number of columns in B // C = output matrix = A-B (m x n) int i, j; for (i=0;i<m;i++) for(j=0;j<n;j++) C[n*i+j]=A[n*i+j]-B[n*i+j]; } static void matrix_transpose(float* A, int m, int n, float* C) // Matrix Transpose Routine { // A = input matrix (m x n) // m = number of rows in A // n = number of columns in A // C = output matrix = the transpose of A (n x m) int i, j; for (i=0;i<m;i++) for(j=0;j<n;j++) C[m*j+i]=A[n*i+j]; } static int matrix_inversion(float* A, int n, float* AInverse) // Matrix Inversion Routine { // A = input matrix (n x n) // n = dimension of A // AInverse = inverted matrix (n x n) // This function inverts a matrix based on the Gauss Jordan method. // The function returns 1 on success, 0 on failure. int i, j, iPass, imx, icol, irow; float det, temp, pivot, factor; float* ac = (float*)calloc(n*n, sizeof(float)); det = 1; for (i = 0; i < n; i++) { for (j = 0; j < n; j++) { AInverse[n*i+j] = 0; ac[n*i+j] = A[n*i+j]; } AInverse[n*i+i] = 1; } // The current pivot row is iPass. // For each pass, first find the maximum element in the pivot column. for (iPass = 0; iPass < n; iPass++) { imx = iPass; for (irow = iPass; irow < n; irow++) { if (fabs(A[n*irow+iPass]) > fabs(A[n*imx+iPass])) imx = irow; } // Interchange the elements of row iPass and row imx in both A and AInverse. if (imx != iPass) { for (icol = 0; icol < n; icol++) { temp = AInverse[n*iPass+icol]; AInverse[n*iPass+icol] = AInverse[n*imx+icol]; AInverse[n*imx+icol] = temp; if (icol >= iPass) { temp = A[n*iPass+icol]; A[n*iPass+icol] = A[n*imx+icol]; A[n*imx+icol] = temp; } } } // The current pivot is now A[iPass][iPass]. // The determinant is the product of the pivot elements. pivot = A[n*iPass+iPass]; det = det * pivot; if (det == 0) { free(ac); return 0; } for (icol = 0; icol < n; icol++) { // Normalize the pivot row by dividing by the pivot element. AInverse[n*iPass+icol] = AInverse[n*iPass+icol] / pivot; if (icol >= iPass) A[n*iPass+icol] = A[n*iPass+icol] / pivot; } for (irow = 0; irow < n; irow++) { // Add a multiple of the pivot row to each row. The multiple factor // is chosen so that the element of A on the pivot column is 0. if (irow != iPass) factor = A[n*irow+iPass]; for (icol = 0; icol < n; icol++) { if (irow != iPass) { AInverse[n*irow+icol] -= factor * AInverse[n*iPass+icol]; A[n*irow+icol] -= factor * A[n*iPass+icol]; } } } } free(ac); return 1; } static void matrix_print(float* A, int m, int n) // Matrix print. { // A = input matrix (m x n) // m = number of rows in A // n = number of columns in A int i, j; for (i=0;i<m;i++) { printf("| "); for(j=0;j<n;j++) { printf("%7.3f ", A[n*i+j]); } printf("|￦n"); } } // n states // m inputs // r outputs #define n 2 #define m 1 #define r 1 float kalman_updat_(float gyroscope_rate, float accelerometer_angle) { // A is an n by n matrix // B is an n by m matrix // C is an r by n matrix // Sz is an r by r matrix // Sw is an n by n matrix // xhat is an n by 1 vector // P is an n by n matrix // y is an r by 1 vector // u is an m by 1 vector // Constants. static float A[n][n] = {{1.0, -0.019968}, {0.0, 1.0}}; static float B[n][m] = {{0.019968}, {0.0}}; static float C[r][n] = {{1.0, 0.0}}; static float Sz[r][r] = {{17.2}}; static float Sw[n][n] = {{0.005, 0.005}, {0.005, 0.005}}; // Persistant states. static float xhat[n][1] = {{0.0}, {0.0}}; static float P[n][n] = {{0.005, 0.005}, {0.005, 0.005}}; // Inputs. float u[m][m]; // Gyroscope rate. float y[m][m]; // Accelerometer angle. // Temp values. float AP[n][n]; // This is the matrix A*P float CT[n][r]; // This is the matrix C' float APCT[n][r]; // This is the matrix A*P*C' float CP[r][n]; // This is the matrix C*P float CPCT[r][r]; // This is the matrix C*P*C' float CPCTSz[r][r]; // This is the matrix C*P*C'+Sz float CPCTSzInv[r][r]; // This is the matrix inv(C*P*C'+Sz) float K[n][r]; // This is the Kalman gain. float Cxhat[r][1]; // This is the vector C*xhat float yCxhat[r][1]; // This is the vector y-C*xhat float KyCxhat[n][1]; // This is the vector K*(y-C*xhat) float Axhat[n][1]; // This is the vector A*xhat float Bu[n][1]; // This is the vector B*u float AxhatBu[n][1]; // This is the vector A*xhat+B*u float AT[n][n]; // This is the matrix A' float APAT[n][n]; // This is the matrix A*P*A' float APATSw[n][n]; // This is the matrix A*P*A'+Sw float KC[n][n]; // This is the matrix K*C float KCP[n][n]; // This is the matrix K*C*P float KCPAT[n][n]; // This is the matrix K*C*P*A' // Fill in inputs. u[0][0] = gyroscope_rate; y[0][0] = accelerometer_angle; #if 0 // Print various matrices. printf("u =￦n"); matrix_print((float*) u, m, m); printf("y =￦n"); matrix_print((float*) y, m, m); printf("A =￦n"); matrix_print((float*) A, n, n); printf("B =￦n"); matrix_print((float*) B, n, m); printf("State =￦n"); matrix_print((float*) xhat, n, 1); #endif // Updat the state estimate by extrapolating estimate with gyroscope input. // xhat_est = A * xhat + B * u matrix_multiply((float*) A, (float*) xhat, n, n, 1, (float*) Axhat); matrix_multiply((float*) B, (float*) u, n, r, 1, (float*) Bu); matrix_addition((float*) Axhat, (float*) Bu, n, 1, (float*) AxhatBu); #if 0 printf("State Estimate =￦n"); matrix_print((float*) AxhatBu, n, 1); #endif // Compute the innovation. // Inn = y - c * xhat; matrix_multiply((float*) C, (float*) xhat, r, n, 1, (float*) Cxhat); matrix_subtraction((float*) y, (float*) Cxhat, r, 1, (float*) yCxhat); #if 0 printf("Innovation =￦n"); matrix_print((float*) yCxhat, r, 1); #endif // Compute the covariance of the innovation. // s = C * P * C' + Sz matrix_transpose((float*) C, r, n, (float*) CT); matrix_multiply((float*) C, (float*) P, r, n, n, (float*) CP); matrix_multiply((float*) CP, (float*) CT, r, n, r, (float*) CPCT); matrix_addition((float*) CPCT, (float*) Sz, r, r, (float*) CPCTSz); // Compute the kalman gain matrix. // K = A * P * C' * inv(s) matrix_multiply((float*) A, (float*) P, n, n, n, (float*) AP); matrix_multiply((float*) AP, (float*) CT, n, n, r, (float*) APCT); matrix_inversion((float*) CPCTSz, r, (float*) CPCTSzInv); matrix_multiply((float*) APCT, (float*) CPCTSzInv, n, r, r, (float*) K); // Updat he state estimate. // xhat = xhat_est + K * Inn; matrix_multiply((float*) K, (float*) yCxhat, n, r, 1, (float*) KyCxhat); matrix_addition((float*) AxhatBu, (float*) KyCxhat, n, 1, (float*) xhat); // Compute the new covariance of the estimation error. // P = A * P * A' - K * C * P * A' + Sw matrix_transpose((float*) A, n, n, (float*) AT); matrix_multiply((float*) AP, (float*) AT, n, n, n, (float*) APAT); matrix_addition((float*) APAT, (float*) Sw, n, n, (float*) APATSw); matrix_multiply((float*) K, (float*) C, n, r, n, (float*) KC); matrix_multiply((float*) KC, (float*) P, n, n, n, (float*) KCP); matrix_multiply((float*) KCP, (float*) AT, n, n, n, (float*) KCPAT); matrix_subtraction((float*) APATSw, (float*) KCPAT, n, n, (float*) P); // Return the estimate. return xhat[0][0]; } int main(int argc, char **argv) { int i; float gyro_input; float accel_input; float kalman_output; for (i = 0; i < SAMPLE_COUNT; ++i) { // Get the gyro and accelerometer input. gyro_input = sample_data[i][0]; accel_input = sample_data[i][1]; // Updat the Kalman filter and get the output. kalman_output = kalman_updat(gyro_input, accel_input); // Print out input data and the kalman output. printf("%d gyro=%7.3f deg/sec accel=%7.3f degrees kalman_output=%5.1f degrees￦n", i, gyro_input, accel_input, kalman_output); } return 0; }