Exemple #1
0
// 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 householder(a *matrix.DenseMatrix) *matrix.DenseMatrix {
	m := a.Rows()
	s := sign(a.Get(0, 0))
	e := unitVector(m)
	u := matrix.Sum(a, matrix.Scaled(e, a.TwoNorm()*s))
	v := matrix.Scaled(u, 1/u.Get(0, 0))
	// (error checking skipped in this solution)
	prod, _ := v.Transpose().TimesDense(v)
	β := 2 / prod.Get(0, 0)

	prod, _ = v.TimesDense(v.Transpose())
	return matrix.Difference(matrix.Eye(m), matrix.Scaled(prod, β))
}
Exemple #3
0
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)
}