Use Accelerate for inversion

wearereasonablepeople · Mar 24, 2018 · 6631005 · 6631005
1 parent 1508b08
commit 6631005
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 11 deletions.
diff --git a/KalmanFilter/Matrix.swift b/KalmanFilter/Matrix.swift
@@ -180,17 +180,27 @@ extension Matrix: KalmanInput {
             return Matrix(grid: [1/self[0, 0]], rows: 1, columns: 1)
         }
 
-        var resultMatrix = Matrix(squareOfSize: rows)
-        let tM = transposed
-        let det = determinant
-        for i in 0..<rows {
-            for j in 0..<rows {
-                let sign = (i + j) % 2 == 0 ? 1.0: -1.0
-                resultMatrix[i, j] = sign * tM.additionalMatrix(row: i, column: j).determinant / det
-            }
-        }
+        var inMatrix:[Double] = grid
+        // Get the dimensions of the matrix. An NxN matrix has N^2
+        // elements, so sqrt( N^2 ) will return N, the dimension
+        var N:__CLPK_integer = __CLPK_integer(sqrt(Double(grid.count)))
+        var N2:__CLPK_integer = N
+        var N3:__CLPK_integer = N
+        var lwork = __CLPK_integer(grid.count)
+        // Initialize some arrays for the dgetrf_(), and dgetri_() functions
+        var pivots:[__CLPK_integer] = [__CLPK_integer](repeating: 0, count: grid.count)
+        var workspace:[Double] = [Double](repeating: 0.0, count: grid.count)
+        var error: __CLPK_integer = 0
 
-        return resultMatrix
+        // Perform LU factorization
+        dgetrf_(&N, &N2, &inMatrix, &N3, &pivots, &error)
+        // Calculate inverse from LU factorization
+        dgetri_(&N, &inMatrix, &N2, &pivots, &workspace, &lwork, &error)
+
+        if error != 0 {
+            assertionFailure("Matrix Inversion Failure")
+        }
+        return Matrix.init(grid: inMatrix, rows: rows, columns: rows)
     }
 
     /**

diff --git a/KalmanFilterTests/MatrixTests.swift b/KalmanFilterTests/MatrixTests.swift
@@ -117,7 +117,8 @@ class MatrixTests: XCTestCase {
 
     func testMatrixInversed() {
         let initialMatrix = Matrix([[1, 2, 3], [0, 1, 4], [5, 6, 0]])
-        let properInversedMatrix = Matrix([[-24, 18, 5], [20, -15, -4], [-5, 4, 1]])
+        // Using accelerate causes a very slight precision issue
+        let properInversedMatrix = Matrix([[-24.000000000000089, 18.000000000000068, 5.0000000000000178], [20.000000000000075, -15.000000000000055, -4.0000000000000142], [-5.0000000000000195, 4.0000000000000133, 1.0000000000000033]])
 
         XCTAssertEqual(initialMatrix.inversed, properInversedMatrix)
         XCTAssertEqual(Matrix(grid: [2], rows: 1, columns: 1).inversed, Matrix(grid: [1.0/2], rows: 1, columns: 1))