kalman filter in go

main.go
package main

import (
    "fmt"
    "math"
    "gonum.org/v1/gonum/mat"
)

func main() {
    // Define system parameters
    A := mat.NewDense(2, 2, []float64{1, 1, 0, 1})
    B := mat.NewDense(2, 1, []float64{0, 1})
    H := mat.NewDense(1, 2, []float64{1, 0})
    Q := mat.NewDiagonal(2, []float64{0.1, 0.1})
    R := mat.NewDiagonal(1, []float64{1})

    // Define initial state
    x := mat.NewVecDense(2, []float64{0, 0})
    P := mat.NewDense(2, 2, []float64{1, 0, 0, 1})

    // Define measurements and control inputs
    z := mat.NewVecDense(1, []float64{math.Sin(0.2)})
    u := mat.NewVecDense(1, []float64{0.1})

    // Initialize filter
    kf := newKalmanFilter(A, B, H, Q, R, x, P)

    // Run filter for 50 iterations
    for i := 0; i < 50; i++ {
        // Predict new state
        kf.predict(u)

        // Update state with measurement
        kf.update(z)

        // Print estimated state
        fmt.Printf("x = %0.3f, %0.3f\n", kf.x.At(0, 0), kf.x.At(1, 0))
    }
}

type kalmanFilter struct {
    A *mat.Dense
    B *mat.Dense
    H *mat.Dense
    Q *mat.Dense
    R *mat.Dense

    x *mat.VecDense
    P *mat.Dense
}

func newKalmanFilter(A, B, H, Q, R *mat.Dense, x *mat.VecDense, P *mat.Dense) *kalmanFilter {
    return &kalmanFilter{
        A: A,
        B: B,
        H: H,
        Q: Q,
        R: R,
        x: x,
        P: P,
    }
}

func (kf *kalmanFilter) predict(u *mat.VecDense) {
    // Predict new state estimate
    x := kf.x
    x = mat.NewVecDense(x.Len(), nil)
    x.MulVec(kf.A, kf.x)
    if u != nil {
        x.MulVec(kf.B, u)
    }
    kf.x = x

    // Predict new error covariance
    P := kf.P
    P = mat.NewDense(P.Rows(), P.Cols(), nil)
    tmp1 := mat.NewDense(P.Rows(), P.Cols(), nil)
    tmp2 := mat.NewDense(P.Rows(), P.Cols(), nil)
    tmp1.Mul(kf.A, kf.P)
    tmp2.Mul(tmp1, kf.A.T())
    P.Add(tmp2, kf.Q)
    kf.P = P
}

func (kf *kalmanFilter) update(z *mat.VecDense) {
    // Compute Kalman Gain
    K := mat.NewDense(kf.P.Rows(), kf.H.T().Cols(), nil)
    tmp1 := mat.NewDense(kf.P.Rows(), kf.H.T().Cols(), nil)
    tmp2 := mat.NewDense(kf.H.Cols(), kf.P.Cols(), nil)
    tmp1.Mul(kf.P, kf.H.T())
    tmp2.Mul(kf.H, tmp1)
    tmp2.Add(tmp2, kf.R)
    tmp2.Inverse(tmp2)
    K.Mul(tmp1, tmp2)

    // Update state estimate
    x := kf.x
    tmp3 := mat.NewVecDense(z.Len(), nil)
    tmp3.MulVec(kf.H, x)
    tmp3.SubVec(z, tmp3)
    tmp4 := mat.NewVecDense(x.Len(), nil)
    tmp4.MulVec(K, tmp3)
    x.AddVec(x, tmp4)
    kf.x = x

    // Update error covariance
    P := kf.P
    tmp5 := mat.NewDense(P.Rows(), P.Cols(), nil)
    tmp6 := mat.NewDense(P.Rows(), kf.H.Cols(), nil)
    tmp5.Mul(K, kf.H)
    tmp6.Sub(mat.NewDense(tmp5.Rows(), P.Cols(), nil).Copy(tmp5), mat.NewDense(tmp5.Rows(), P.Cols(), nil).Copy(mat.NewDense(tmp5.Rows(), P.Cols(), nil).Mul(K, kf.H.T())))
    P.Mul(tmp6, P)
    kf.P = P
}
2875 chars
115 lines