Add VanLoan's c2d method

c2d.go

@@ -0,0 +1,52 @@
+package gokalman
+
+import "github.com/gonum/matrix/mat64"
+
+// VanLoan computes the F and Q matrices from the provided CT system A, Γ, W and
+// the sampling rate Δt.
+func VanLoan(A, Γ, W mat64.Matrix, Δt float64) (*mat64.Dense, *mat64.SymDense) {
+	var ΓW, ΓWΓ, Ap mat64.Dense
+	ΓW.Mul(Γ, W)
+	ΓWΓ.Mul(&ΓW, Γ.T())
+	ΓWΓ.Scale(Δt, &ΓWΓ)
+	Ap.Scale(Δt, A)
+	// Find the size of the M matrix.
+	rA, cA := A.Dims()
+	r1, c1 := ΓWΓ.Dims()
+	rM := rA + cA
+	cM := cA + c1
+	M := mat64.NewDense(rM, cM, nil)
+
+	// Populate M
+	for i := 0; i < rA; i++ {
+		for j := 0; j < cA; j++ {
+			M.Set(i, j, -Ap.At(i, j))
+			M.Set(i+rA, j+cA, Ap.T().At(i, j))
+		}
+	}
+	for i := 0; i < r1; i++ {
+		for j := 0; j < c1; j++ {
+			M.Set(i, j+cA, ΓWΓ.At(i, j))
+		}
+	}
+
+	// Compute exponential
+	var expM mat64.Dense
+	expM.Exp(M)
+	reM, ceM := expM.Dims()
+
+	// Extract F transpose (and F^-1*Q) knowing it has the same size as A.
+	F := mat64.NewDense(rA, cA, nil)
+	F1Q := mat64.NewDense(rA, cA, nil)
+	for i := 0; i < rA; i++ {
+		for j := 0; j < cA; j++ {
+			F1Q.Set(i, j, expM.At(i, ceM-cA+j))
+			F.Set(i, j, expM.At(reM-rA+i, ceM-cA+j))
+		}
+	}
+	F = mat64.DenseCopyOf(F.T())
+	var Q mat64.Dense
+	Q.Mul(F, F1Q)
+	QSym, _ := AsSymDense(&Q)
+	return F, QSym
+}

c2d_test.go

@@ -0,0 +1,24 @@
+package gokalman
+
+import (
+	"testing"
+
+	"github.com/gonum/matrix/mat64"
+)
+
+func TestVanLoan(t *testing.T) {
+	A := mat64.NewDense(2, 2, []float64{0, 1, 0, 0})
+	Γ := mat64.NewDense(2, 1, []float64{0, 1})
+	W := mat64.NewDense(1, 1, []float64{1})
+	F, Q := VanLoan(A, Γ, W, 0.1)
+	Fexp := mat64.NewDense(2, 2, []float64{1, 0.1, 0, 1})
+	Qexp := mat64.NewSymDense(2, []float64{0.0003, 0.005, 0.005, 0.1})
+
+	if !mat64.EqualApprox(F, Fexp, 1e-3) {
+		t.Fatal("F incorrectly computed")
+	}
+
+	if !mat64.EqualApprox(Q, Qexp, 1e-3) {
+		t.Fatal("Q incorrectly computed")
+	}
+}