Class: CalcSat::Kalman

Inherits:
Object
  • Object
show all
Includes:
Numo
Defined in:
lib/calc_sat.rb

Instance Attribute Summary collapse

Instance Method Summary collapse

Instance Attribute Details

#ft0 ⇒ Object

Returns the value of attribute ft0.



111
112
113
 
# File 'lib/calc_sat.rb', line 111

def ft0
  @ft0
end

#gt0 ⇒ Object

Returns the value of attribute gt0.



111
112
113
 
# File 'lib/calc_sat.rb', line 111

def gt0
  @gt0
end

#ht0 ⇒ Object

Returns the value of attribute ht0.



111
112
113
 
# File 'lib/calc_sat.rb', line 111

def ht0
  @ht0
end

#pt1 ⇒ Object

Returns the value of attribute pt1.



111
112
113
 
# File 'lib/calc_sat.rb', line 111

def pt1
  @pt1
end

#qt0 ⇒ Object

Returns the value of attribute qt0.



111
112
113
 
# File 'lib/calc_sat.rb', line 111

def qt0
  @qt0
end

#rt0 ⇒ Object

Returns the value of attribute rt0.



111
112
113
 
# File 'lib/calc_sat.rb', line 111

def rt0
  @rt0
end

#xht1 ⇒ Object

Returns the value of attribute xht1.



111
112
113
 
# File 'lib/calc_sat.rb', line 111

def xht1
  @xht1
end

Instance Method Details

#et0(zt0) ⇒ Object 



132
133
134
 
# File 'lib/calc_sat.rb', line 132

def et0(zt0)
  zt0 - @ht0.dot(@xht0)
end

#kt0 ⇒ Object 



140
141
142
 
# 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) ⇒ Object 



127
128
129
130
 
# File 'lib/calc_sat.rb', line 127

def observe(zt0, ut0=nil)
  predict(ut0)
  update(zt0)
end

#predict(ut0) ⇒ Object 



113
114
115
116
117
118
 
# 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 ⇒ Object 



136
137
138
 
# File 'lib/calc_sat.rb', line 136

def st0
  @rt0 + @ht0.dot(@pt0).dot(@ht0.transpose)
end

#update(zt0) ⇒ Object 



120
121
122
123
124
125
 
# 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