Fixed bug in EKF residual function.

Comment was wrong, it normalizes to [-pi, pi].

Also, code didn't handle cases where a-b > 2Pi
This commit is contained in:
Roger Labbe 2015-07-15 19:03:59 -07:00
parent 7827a4df2d
commit 335df7f42d
3 changed files with 160 additions and 61 deletions

View File

@ -3433,7 +3433,7 @@
"source": [
"def normalize_angle(x):\n",
" x = x % (2 * np.pi) # force in range [0, 2 pi)\n",
" if x > np.pi: # move to [-pi, pi]\n",
" if x > np.pi: # move to [-pi, pi)\n",
" x -= 2 * np.pi\n",
" return x"
]

File diff suppressed because one or more lines are too long

View File

@ -12,7 +12,7 @@ sys.path.insert(0,'./code') # allow us to import book_format
def test_filterpy_version():
import filterpy
min_version = [0,0,22]
min_version = [0,0,25]
v = filterpy.__version__
tokens = v.split('.')
for i,v in enumerate(tokens):