func Kalman(W, R, Q, x0, P0, A, B, H matrix.Matrix) (k *ExtKalmanFilter) {
f := func(x, u matrix.Matrix) matrix.Matrix {
// f(x) = Ax + Bu
return matrix.Sum(matrix.Product(A, x), matrix.Product(B, u))
}
dfdx := func(x, u matrix.Matrix) matrix.Matrix {
// df/dx = A
return A
}
h := func(x matrix.Matrix) matrix.Matrix {
// h(x) = H*x
return matrix.Product(A, x)
}
dhdx := func(x matrix.Matrix) matrix.Matrix {
// dh/dx = H
return H
}
return ExtendedKalman(W, R, Q, x0, P0, f, dfdx, h, dhdx, nil)
}
// y: Measurments
// u: Actuator thrust
func (k *ExtKalmanFilter) Step(y, u matrix.Matrix) (x matrix.Matrix) {
var y_ = k.h(k.x)
var H = k.dhdx(k.x)
var Ht = matrix.Transpose(H)
// -----------------
// Estimation
// Kalman gain matrix
// K = P * H^t * (H*P*Ht + R)⁻¹
var inv = matrix.Inverse(matrix.Sum(matrix.Product(H, k.mP, Ht), k.mR))
var K = matrix.Product(k.mP, Ht, inv)
var Kt = matrix.Transpose(K)
// State estimation update
// x_ = x + K * (y - h(x))
var ydiff, _ = y.Minus(y_)
if k.normalize_ydiff != nil {
ydiff = k.normalize_ydiff(ydiff)
}
var x_ = matrix.Sum(k.x, matrix.Product(K, ydiff))
// Error covariance update
// P_ = (I - K*H) * P * (I - K*H)^t + K*R*K^t
var IKH, _ = matrix.Eye(H.Cols()).Minus(matrix.Product(K, H))
var IKHt = matrix.Transpose(IKH)
var P_ = matrix.Sum(matrix.Product(IKH, k.mP, IKHt), matrix.Product(K, k.mR, Kt))
// -----------------
// Propagation
var F = k.dfdx(x_, u)
var Ft = matrix.Transpose(F)
// State estimation propagation
// x(k+1) = A*x_(k) + B*u
k.x = k.f(x_, u)
// Error covariance propagation
// P(k+1) = A*P_*A^t + W*Q*W^t
k.mP = matrix.Sum(matrix.Product(F, P_, Ft), k.cWQWt)
return k.x
}
func ExtendedKalman(
W, R, Q, x0, P0 matrix.Matrix,
f, dfdx func(matrix.Matrix, matrix.Matrix) matrix.Matrix,
h, dhdx, normalize_ydiff func(matrix.Matrix) matrix.Matrix) (k *ExtKalmanFilter) {
k = new(ExtKalmanFilter)
k.mW = W
k.mR = R
k.mQ = Q
k.x = x0
k.mP = P0
k.f = f
k.dfdx = dfdx
k.h = h
k.dhdx = dhdx
k.normalize_ydiff = normalize_ydiff
// Pre calculated constant
k.cWQWt = matrix.Product(W, Q, matrix.Transpose(W))
return
}
func main() {
// Beacon positions
var beaconA = newPos(0.0, 0.0)
var beaconB = newPos(0.0, 2000.0)
var beaconC = newPos(3000.0, 1000.0)
// First state
var x0 matrix.Matrix = matrix.MakeDenseMatrix([]float64{200, 0, 200, 0}, 4, 1)
var P0 matrix.Matrix = matrix.Diagonal([]float64{10000, 100, 10000, 100})
fmt.Printf(">init:\n")
fmt.Printf("x0:\n%v\n\n", x0)
fmt.Printf("P0:\n%v\n\n", P0)
// x(k+1) = Ax + Bu + Ww
var W matrix.Matrix = matrix.MakeDenseMatrix([]float64{
0.0, 0.0,
1.0, 0.0,
0.0, 0.0,
0.0, 1.0}, 4, 2)
fmt.Printf("> noise\n")
fmt.Printf("W:\n%v\n\n", W)
// Design
var d = 0.01 * 0.01
var Q matrix.Matrix = matrix.Diagonal([]float64{d, d})
var R matrix.Matrix = matrix.Diagonal([]float64{d, d, d})
fmt.Printf(">Design\n")
fmt.Printf("Q:\n%v\n\n", Q)
fmt.Printf("R:\n%v\n\n", R)
// Init filter
var u matrix.Matrix = matrix.MakeDenseMatrix([]float64{0, 0}, 2, 1)
var y matrix.Matrix = matrix.MakeDenseMatrix([]float64{1000, 1000, 3000}, 3, 1)
// df(x,u)/dx
dfdx := func(x, u matrix.Matrix) matrix.Matrix {
// The process is independent of possition, df/dx = A
T := 1.0
A := matrix.MakeDenseMatrix([]float64{
1.0, T, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, T,
0.0, 0.0, 0.0, 1.0}, 4, 4)
return A
}
// Estimated movement of robot given state x
f := func(x, u matrix.Matrix) matrix.Matrix {
// x(k+1) = Ax + Bu
var A = dfdx(x, u)
return matrix.Product(A, x)
// Commented out: take motor gain in to account:
/*
var B matrix.Matrix = matrix.MakeDenseMatrix([]float64{
0.0, 0.0,
0.0, 0.0,
0.0, 0.0,
0.0, 0.0}, 4, 2)
return matrix.Sum( matrix.Product(A, x), matrix.Product(B, u))
*/
}
// dh(x,u)/dx
dhdx := func(x matrix.Matrix) matrix.Matrix {
// Linearisation of H around x(k)
robot := newPos(x.Get(0, 0), x.Get(2, 0))
denA := math.Sqrt(math.Pow(robot.X-beaconA.X, 2.0) + math.Pow(robot.Y-beaconA.Y, 2.0))
denB := math.Sqrt(math.Pow(robot.X-beaconB.X, 2.0) + math.Pow(robot.Y-beaconB.Y, 2.0))
denC := math.Sqrt(math.Pow(robot.X-beaconC.X, 2.0) + math.Pow(robot.Y-beaconC.Y, 2.0))
H := matrix.MakeDenseMatrix([]float64{
(robot.X - beaconA.X) / denA, 0.0, (robot.Y - beaconA.Y) / denA, 0.0,
(robot.X - beaconB.X) / denB, 0.0, (robot.Y - beaconB.Y) / denB, 0.0,
(robot.X - beaconC.X) / denC, 0.0, (robot.Y - beaconC.Y) / denC, 0.0}, 3, 4)
return H
}
// Measure estimate given state x
h := func(x matrix.Matrix) matrix.Matrix {
var robot = newPos(x.Get(0, 0), x.Get(2, 0))
var y matrix.Matrix = matrix.MakeDenseMatrix([]float64{
math.Sqrt(math.Pow(robot.X-beaconA.X, 2.0) + math.Pow(robot.Y-beaconA.Y, 2.0)),
math.Sqrt(math.Pow(robot.X-beaconB.X, 2.0) + math.Pow(robot.Y-beaconB.Y, 2.0)),
math.Sqrt(math.Pow(robot.X-beaconC.X, 2.0) + math.Pow(robot.Y-beaconC.Y, 2.0))}, 3, 1)
return y
}
// State variable
var x matrix.Matrix
// Initalize kalman filter
var filter = extkalman.ExtendedKalman(W, R, Q, x0, P0, f, dfdx, h, dhdx)
// Run filter
fmt.Printf("Running:\n")
for {
x = filter.Step(y, u)
fmt.Printf("x:\n%v\n\n", x)
}
}