Switched estimate sub to StateArray. Implemented new initialization procedure.