class CalcSat::Kalman

Attributes

ft0[RW]
gt0[RW]
ht0[RW]
pt1[RW]
qt0[RW]
rt0[RW]
xht1[RW]

Public Instance Methods

et0(zt0) click to toggle source
# File lib/calc_sat.rb, line 132
def et0(zt0)
  zt0 - @ht0.dot(@xht0)
end
kt0() click to toggle source
# File lib/calc_sat.rb, line 140
def kt0
  @pt0.dot(@ht0.transpose).dot(Matrix[*st0.to_a].inv.to_a)
end
observe(zt0, ut0=nil) click to toggle source
# File lib/calc_sat.rb, line 127
def observe(zt0, ut0=nil)
  predict(ut0)
  update(zt0)
end
predict(ut0) click to toggle source
# File lib/calc_sat.rb, line 113
def predict(ut0)
  ut0 = 0 if not ut0
  @xht0 = @ft0.dot(@xht1) + ut0
  @pt0 = @ft0.dot(@pt1).dot(@ft0.transpose)+@gt0.dot(@qt0).dot(@gt0.transpose)
  [@xht0, @pt0]
end
st0() click to toggle source
# File lib/calc_sat.rb, line 136
def st0
  @rt0 + @ht0.dot(@pt0).dot(@ht0.transpose)
end
update(zt0) click to toggle source
# File lib/calc_sat.rb, line 120
def update(zt0)
  @xht1 = @xht0 + kt0.dot(et0(zt0))
  ktht = kt0.dot(@ht0)
  @pt1 = (Int32.eye(*ktht.shape) - ktht).dot(@pt0)
  [@xht1, @pt1]
end