This is the best debug method: the log with the WENC distances clarifies everything since you can check a real trajectory. See this and specially this.
