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