Added 3-sigma plots. Turns out the rotational uncertainty never gets updated. It just grows forever and eventually causes the filter to diverge.