예제 #1
0
파일: kalman.go 프로젝트: postfix/extkalman
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)
}
예제 #2
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
}
예제 #3
0
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)
	}

}