Testing a simple alpha-beta filter concept for estimating target positional changes during mid-course guidance phase of an interceptor missile.
def FLINT_Kalman__ab_filter( self, aPos, aVelocity, dTime ):
"""
FLINT_Kalman__ab_filter( aPos, aVelocity, dTime )
A test case for using simple alpha-beta filter to generate
estimated target positional updates during mid-course guidance.
Usage:
aPos = target position vector
aVelocity = target velocity vector
dTime = time constant for testing (TGO?)
"""
xk_1 = math.Vector3( aPos )
vk_1 = math.Vector3( aVelocity ).NormalizeSafe()
dt = float(dTime)
a = 0.85
b = 0.005
# signal input - x_measured
xm = math.Vector3( aPos )
# predicted position
xk = xk_1 + ( vk_1 * dt )
# angular position estimation
rk = xm - xk
xk += rk * a
# velocity estimation
#vk = vk_1
#vk += ( rk * b ) * (1 / dt)
return xk