Tested the first AUKF Implementation. The filter works well for the gaussianK, but not so well for the stageK. Next step is to include K in the state vector.