It seems to me that through these files in attachment of which they are in the subject you will be able to soluze your problem. Also please look at links.
-COOPERATIVE TARGET TRACKING USING MOBILE ROBOTS by ...
cres.usc.edu/Research/files/bjung_thesis.pdf
-How to Protect Your Personal Data—and Humanity—From the ...
In principle, the only way to turn an unobservable system into an observable one is to add sensors (as long as they provide new information. In the linear case it means that they are not linearly dependent of existing measurements). This should solve the "classical" observability problem (that of being able to find the initial state from the output and the inputs) which is what we find the in the field of control engineering.
A related problem is to define a reconstruction space from the recorded variables. Whenever there is a global diffeomorphism connecting the state space to the reconstructed space, we say that the original system is observable. In this case, lack of observability arises from the presence of singularities in the jacobian matrix of the map between the original and reconstructed space. In such cases, it is generally possible to remove such singularities by increasing the dimension of the reconstructed space. This is commonly done in nonlinear dynamics.
Here are some references of interest to deal with control of nonlinear systems, control of industrial manipulators, observability considerations and observer design:
Observation matrix & Dynamic Model Identification for Industrial Robots - https://uni-obuda.hu/journal/Voung_Ang_21.pdf