What is the best way to implement a Kalman Filter in Simulink?

Hi,

I'm currently developing Invariant Kalman Filter algorithms for use in Simscape Multibody models and therefore need to be able to run recursive filters in simulations. The attached photo shows my current method for implementing a state estimator but I have to do a lot of legwork to get it to compile correctly in Simulink. This involves using IC and Transport Delay blocks with a MATLAB Function holding the required functions.

I was wondering if anyone would be able to suggest a more elegant solution?

Many thanks,

Joe