func qr(a *matrix.DenseMatrix) (q, r *matrix.DenseMatrix) {
m := a.Rows()
n := a.Cols()
q = matrix.Eye(m)
last := n - 1
if m == n {
last--
}
for i := 0; i <= last; i++ {
// (copy is only for compatibility with an older version of gomatrix)
b := a.GetMatrix(i, i, m-i, n-i).Copy()
x := b.GetColVector(0)
h := matrix.Eye(m)
h.SetMatrix(i, i, householder(x))
q, _ = q.TimesDense(h)
a, _ = h.TimesDense(a)
}
return q, a
}
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, β))
}
// 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
}