Android2013. 4. 1. 15:08

 

칼만필터 적용 과정중에 구현한 행렬 연산 소스입니다.

 

안드로이드에서 만든건데 당연히 자바에서도 되겠네요

 

4x4 행렬의 덧셈, 뺄셈, 곱셈을 이중 for문으로 계산해서 반환해줍니다.

 

행렬 연산하시는분 참고하세요~

 

 함수 정의 부

 // 4x4 행렬 덧셈
  double[][] matrix_plus(double[][] A, double[][]B) {
   double[][] C = new double[4][4];
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
      C[i][j] = A[i][j] + B[i][j];
     }
     
    }
   return C;
  }
  // 1x4 행렬 덧셈
  double[] matrix_14plus14(double[] A, double[] B) {
   double[] C = new double[4];
   for(int i=0; i<4; i++) {
      C[i] = A[i] + B[i];
    }
   return C;
  }
  // 4x4 행렬 뺄셈
  double[][] matrix_minus(double[][] A, double[][]B) {
   double[][] C = new double[4][4];
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
      C[i][j] = A[i][j] - B[i][j];
     }
     
    }
   return C;
  }
  // 1x4 행렬 뺄셈
  double[] matrix_14minus14(double[] A, double[]B) {
   double[] C = new double[4];
   for(int i=0; i<4; i++) {
      C[i] = A[i] - B[i];
     }
   return C;
  }
  // 4x4 행렬 곱셈
  double[][] matrix_mul(double[][] A, double[][]B) {
   double[][] C = new double[4][4];
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
     for(int k=0; k<4; k++) {
      C[i][j] += A[i][k] * B[k][j];
     }
     
    }
   }
   return C;
  }
  // 1x4 * 4x4 행렬 곱셈
  double[] matrix_14mul44(double[] A, double[][]B) {
   double[] C = new double[4];
   for(int i=0; i<4; i++) {
     for(int j=0; j<4; j++) {
      C[i] += A[j] * B[j][i];
     }
   }
   return C;
  }
  // 4x4 행렬 전치
  double[][] matrix_transed(double[][] A) {
   double[][] C = new double[4][4];
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
      C[i][j] = A[j][i];
    }
   }
   
   return C;
  }
  // 4x4 행렬 역행렬
  double[][] matrix_inverse(double[][] A) {
   
   double[][] C = new double[4][4];
   
   Matrix mat = new Matrix(4,4);     // Matrix.java는 파일첨부
   for(int i=0; i<4; i++) {
    for(int j=0; j<4; j++) {
     mat.values[i][j] = A[i][j];
    }
   }
   Matrix.Invers inv = mat.new Invers();
   C = inv.Invers(mat);
   
   return C;
  }

 

밑에 소스는 구글링에서 퍼온건데.. 기억이 안나네요 ㅠㅠ (아시는분 댓글 달아주시면 바로 출처 올리겠습니다~)

 

4x4 역행렬 연산에 쓰이는 Matrix 클래스입니다.

 

 Matrix.java

 public class Matrix {
 public int row; // 행 전역변수 선언
 public int col; // 열 전역변수 선언
 public double[][] values; // 2차원 배열 행렬 전역변수 선언

 public Matrix(int row, int col) { // 생성자 Matrix 생성
  this.row = row; // 행의 값을 받는다.
  this.col = col; // 열의 값을 받는다.
  values = new double[row][col];// 전역변수 values에 row값과 col값을 넣어준다.
 }

 class Invers // Invers 메소드
 {
  determi det = new determi(); // 행렬식을 이용하기 위해 불러옴

  public double[][] Invers(Matrix mat) {
   Matrix inv = new Matrix(mat.row, mat.col); // 역행렬을 저장한 행렬 생성
   Matrix cofactor = new Matrix(mat.row, mat.col);
   double de = (double) 1 / det.data(mat);

   if (det.data(mat) == 0) // 행렬식이 0일 경우
   {
    System.out.println("역행렬이  있지 않습니다.");// 메세지 툴력
    System.exit(0); // 그리고 시스템 exit
   }

   for (int i = 0; i < mat.row; i++) { // 0부터 행 값까지 반복
    for (int j = 0; j < mat.col; j++) {// 0부터 열 값까지 반복
     cofactor.values[i][j] = Mino(mat, i, j);
     inv.values[j][i] = de * Math.pow(-1, i + j)
       * cofactor.values[i][j];
    }
   }
   return inv.values;// inv를 리턴한다.
  }
  
  public double Mino(Matrix mat, int r, int c) {// 여인수전개를 위한 메소드
   double cof;
   Matrix minor = new Matrix(mat.row - 1, mat.col - 1);
   int k = 0;
   if (k == r) {
    k++;
   } // C_ij의 소행렬은 i행j열을 제외하므로 이런 if를 쓴다.
   for (int i = 0; i < minor.row; i++) // 0부터 행값까지 반복
   {
    int l = 0; // l은 초기값으로 0
    if (l == c) {
     l++;
    }// l값이 열값과 같다면 l증가
    if (k == r) {
     k++;
    }// k값이 행값과 같다면 k증가
    for (int j = 0; j < minor.col; j++) {
     if (l == c) {
      l++;
     } // 열값과 같아면 l증가
     minor.values[i][j] = mat.values[k][l];
     l++;
    }
    k++;
   }
   cof = det.data(minor);// 소행렬을 구해 반환
   return cof;
  }
 }

 static class determi {
  static int order;
  static double[][] matrix;

  public double data(Matrix mat) // 행렬식을 구할 행렬을 받는다.
  {
   matrix = new double[mat.row][mat.col];
   order = mat.row; // order는 받은 행렬의 행의 수이다.
   for (int i = 0; i < mat.row; i++)// i를 행의수만큼 돌리고
   {
    for (int j = 0; j < mat.col; j++) // j는 열의수만큼 돌려서
    {
     matrix[i][j] = mat.values[i][j]; // matrix에 mat과 같은 값을 넣는다.
    }
   }
   return compute(); // 리턴을 compute함수로 시켜준다.
  }

  public static double compute() // 그럼 여기로 오는데,
  {
   double check[] = new double[20]; // check란 20칸짜리 배열을 만든다.
   double det = 0; // det는 초기화하고
   int row, col = 0; // row와 col도 초기화선언한다.
   for (row = 0; row < order; row++) // row는 받은 행의 수 만큼 돌리고
   {
    check[row] = 1; // 이때 check의 행의수만큼의 index를 1로 지정
   }

   if (order == 1)// 만약 order가 1이면
   {
    det = matrix[0][0]; // det는 a11값이 된다.
   }

   for (row = 0; row < order; row++) // 다시 row를 수만큼 돌림
   {
    check[row] = 0;// row[i]에 0을 집어넣음
    det += matrix[row][0] * Math.pow(-1, row)
      * minor(row, col + 1, check);
    // det를 1열의 수 곱하기 minor함수에서 받은 값으로 온다
    check[row] = 1; // 이렇게 계산 되면 다시 check를 1로 해준다.
   }
   return det; // det 값을 반환
  }

  public static double minor(int Row, int Col, double trans[]) {
   double result = 0;// 결과값으로 초기값을 0을 준다.
   int i = 0;
   if ((order - Col) == 0) {
    return 1;
   }
   for (int row = 0; row < order; row++)// 0부터
   {
    if (trans[row] != 0)// 만약 trans[row]가 0이 아니라면
    {
     trans[row] = 0; // 0을 넣어준다.
     result += matrix[row][Col] * Math.pow(-1, (double) i)
       * minor(row, Col + 1, trans);
     // 여인수전개에 의한 식이다. 재귀함수로 계속 돌리게된다.
     trans[row] = 1; // trans[row]에 1을 넣어준다.
     i++;// i 값 증가
    }
   }
   return result; // result 반환
  }
 }

}


 

 

Posted by 너를위한노래