3b4c5a547c
I was erroneously using a scalar for Q. I made it into a matrix. However, I am pretty sure it is still wrong. Q shouldn't be symmetric.
30 lines
355 B
Python
30 lines
355 B
Python
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Tue May 13 13:10:50 2014
|
|
|
|
@author: RL
|
|
"""
|
|
|
|
# position, velocity (fps)
|
|
pos = 20000
|
|
vel = 200
|
|
|
|
|
|
t = 10
|
|
z = 22060
|
|
|
|
|
|
def predict_dz (vel,t):
|
|
return vel*t
|
|
|
|
|
|
dz = z - pos
|
|
|
|
|
|
print dz - predict_dz(vel,t)
|
|
|
|
|
|
h = 0.1
|
|
vel = vel + h * (dz - predict_dz(vel,t)) / t
|
|
print 'new vel =', vel
|