After struggling with the mathematical formulation of the problem for what has been a long time now, it is finally in a state where it can be converted into code. The importance of not overlooking the accuracy of the math cannot be understated since this uniquely defines the code. Whilst performing this task a number of factors stood out that could impact the success of the implementation. Firstly it is important to note that whilest the distance measurement collapses to a single point on a single wall from the perspective of the plant model, the wall segment has to be determined via the estimator. This means that the distance measurement will also have to be computed on on the estimator in order to give at least a prior indication as to which wall section is most likely providing the respective distance measurement. Secondly, having spoken to Tsang-Tai it is interesting to note his perspective on the topic which is that the EKF is NOT the right tool to use for the job, however that is yet to be determined once we have a better understanding of the system. Watch this space for more updates