% Data for GPS problem part b
m = 5; %number of satellites
N = 100; %number of time steps
T = 72; %lenght of the time step
c = 300000000; %speed of light
sigmatau = 1.000000e-04; %standard deviation of tau 
sigmav = 10; % standard deviation of v 
sigmax = 10000; % standard deviation of x
sigmaw = 10; % standard deviation of w 
sat_pos_1=[1.379e+07 1.357e+07 1.336e+07 1.315e+07 1.294e+07 1.272e+07 1.251e+07 1.23e+07 1.208e+07 1.187e+07 1.166e+07 1.145e+07 1.123e+07 1.102e+07 1.081e+07 1.059e+07 1.038e+07 1.017e+07 9.955e+06 9.742e+06 9.529e+06 9.316e+06 9.103e+06 8.89e+06 8.677e+06 8.464e+06 8.251e+06 8.038e+06 7.826e+06 7.613e+06 7.4e+06 7.187e+06 6.974e+06 6.761e+06 6.548e+06 6.335e+06 6.122e+06 5.909e+06 5.696e+06 5.483e+06 5.271e+06 5.058e+06 4.845e+06 4.632e+06 4.419e+06 4.206e+06 3.993e+06 3.78e+06 3.567e+06 3.354e+06 3.141e+06 2.928e+06 2.716e+06 2.503e+06 2.29e+06 2.077e+06 1.864e+06 1.651e+06 1.438e+06 1.225e+06 1.012e+06 7.994e+05 5.864e+05 3.735e+05 1.606e+05 -5.23e+04 -2.652e+05 -4.781e+05 -6.91e+05 -9.04e+05 -1.117e+06 -1.33e+06 -1.543e+06 -1.756e+06 -1.969e+06 -2.181e+06 -2.394e+06 -2.607e+06 -2.82e+06 -3.033e+06 -3.246e+06 -3.459e+06 -3.672e+06 -3.885e+06 -4.098e+06 -4.311e+06 -4.523e+06 -4.736e+06 -4.949e+06 -5.162e+06 -5.375e+06 -5.588e+06 -5.801e+06 -6.014e+06 -6.227e+06 -6.44e+06 -6.653e+06 -6.866e+06 -7.078e+06 -7.291e+06;2.222e+07 2.204e+07 2.185e+07 2.167e+07 2.149e+07 2.131e+07 2.113e+07 2.095e+07 2.076e+07 2.058e+07 2.04e+07 2.022e+07 2.004e+07 1.985e+07 1.967e+07 1.949e+07 1.931e+07 1.913e+07 1.894e+07 1.876e+07 1.858e+07 1.84e+07 1.822e+07 1.804e+07 1.785e+07 1.767e+07 1.749e+07 1.731e+07 1.713e+07 1.694e+07 1.676e+07 1.658e+07 1.64e+07 1.622e+07 1.604e+07 1.585e+07 1.567e+07 1.549e+07 1.531e+07 1.513e+07 1.494e+07 1.476e+07 1.458e+07 1.44e+07 1.422e+07 1.404e+07 1.385e+07 1.367e+07 1.349e+07 1.331e+07 1.313e+07 1.294e+07 1.276e+07 1.258e+07 1.24e+07 1.222e+07 1.203e+07 1.185e+07 1.167e+07 1.149e+07 1.131e+07 1.113e+07 1.094e+07 1.076e+07 1.058e+07 1.04e+07 1.022e+07 1.003e+07 9.853e+06 9.671e+06 9.489e+06 9.307e+06 9.125e+06 8.943e+06 8.762e+06 8.58e+06 8.398e+06 8.216e+06 8.034e+06 7.852e+06 7.671e+06 7.489e+06 7.307e+06 7.125e+06 6.943e+06 6.761e+06 6.579e+06 6.398e+06 6.216e+06 6.034e+06 5.852e+06 5.67e+06 5.488e+06 5.307e+06 5.125e+06 4.943e+06 4.761e+06 4.579e+06 4.397e+06 4.215e+06;2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07]; 
sat_pos_2=[-2.779e+07 -2.757e+07 -2.736e+07 -2.714e+07 -2.693e+07 -2.671e+07 -2.65e+07 -2.628e+07 -2.607e+07 -2.586e+07 -2.564e+07 -2.543e+07 -2.521e+07 -2.5e+07 -2.478e+07 -2.457e+07 -2.435e+07 -2.414e+07 -2.392e+07 -2.371e+07 -2.35e+07 -2.328e+07 -2.307e+07 -2.285e+07 -2.264e+07 -2.242e+07 -2.221e+07 -2.199e+07 -2.178e+07 -2.157e+07 -2.135e+07 -2.114e+07 -2.092e+07 -2.071e+07 -2.049e+07 -2.028e+07 -2.006e+07 -1.985e+07 -1.963e+07 -1.942e+07 -1.921e+07 -1.899e+07 -1.878e+07 -1.856e+07 -1.835e+07 -1.813e+07 -1.792e+07 -1.77e+07 -1.749e+07 -1.728e+07 -1.706e+07 -1.685e+07 -1.663e+07 -1.642e+07 -1.62e+07 -1.599e+07 -1.577e+07 -1.556e+07 -1.534e+07 -1.513e+07 -1.492e+07 -1.47e+07 -1.449e+07 -1.427e+07 -1.406e+07 -1.384e+07 -1.363e+07 -1.341e+07 -1.32e+07 -1.299e+07 -1.277e+07 -1.256e+07 -1.234e+07 -1.213e+07 -1.191e+07 -1.17e+07 -1.148e+07 -1.127e+07 -1.106e+07 -1.084e+07 -1.063e+07 -1.041e+07 -1.02e+07 -9.983e+06 -9.768e+06 -9.554e+06 -9.339e+06 -9.125e+06 -8.91e+06 -8.696e+06 -8.481e+06 -8.267e+06 -8.052e+06 -7.838e+06 -7.623e+06 -7.409e+06 -7.194e+06 -6.98e+06 -6.765e+06 -6.551e+06;-7.578e+07 -7.596e+07 -7.614e+07 -7.632e+07 -7.65e+07 -7.668e+07 -7.686e+07 -7.704e+07 -7.722e+07 -7.74e+07 -7.758e+07 -7.776e+07 -7.794e+07 -7.812e+07 -7.83e+07 -7.848e+07 -7.866e+07 -7.884e+07 -7.902e+07 -7.92e+07 -7.938e+07 -7.956e+07 -7.974e+07 -7.992e+07 -8.01e+07 -8.028e+07 -8.046e+07 -8.064e+07 -8.082e+07 -8.1e+07 -8.118e+07 -8.136e+07 -8.154e+07 -8.172e+07 -8.19e+07 -8.208e+07 -8.226e+07 -8.244e+07 -8.262e+07 -8.28e+07 -8.298e+07 -8.316e+07 -8.334e+07 -8.352e+07 -8.37e+07 -8.388e+07 -8.406e+07 -8.424e+07 -8.442e+07 -8.46e+07 -8.478e+07 -8.496e+07 -8.514e+07 -8.532e+07 -8.55e+07 -8.568e+07 -8.586e+07 -8.604e+07 -8.622e+07 -8.64e+07 -8.658e+07 -8.676e+07 -8.694e+07 -8.712e+07 -8.73e+07 -8.748e+07 -8.766e+07 -8.784e+07 -8.802e+07 -8.82e+07 -8.838e+07 -8.856e+07 -8.874e+07 -8.892e+07 -8.91e+07 -8.928e+07 -8.946e+07 -8.964e+07 -8.982e+07 -9e+07 -9.018e+07 -9.036e+07 -9.054e+07 -9.072e+07 -9.09e+07 -9.108e+07 -9.126e+07 -9.144e+07 -9.162e+07 -9.18e+07 -9.198e+07 -9.216e+07 -9.234e+07 -9.252e+07 -9.27e+07 -9.288e+07 -9.306e+07 -9.324e+07 -9.342e+07 -9.36e+07;2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07]; 
sat_pos_3=[-7.547e+07 -7.535e+07 -7.522e+07 -7.509e+07 -7.496e+07 -7.484e+07 -7.471e+07 -7.458e+07 -7.446e+07 -7.433e+07 -7.42e+07 -7.407e+07 -7.395e+07 -7.382e+07 -7.369e+07 -7.357e+07 -7.344e+07 -7.331e+07 -7.318e+07 -7.306e+07 -7.293e+07 -7.28e+07 -7.268e+07 -7.255e+07 -7.242e+07 -7.229e+07 -7.217e+07 -7.204e+07 -7.191e+07 -7.179e+07 -7.166e+07 -7.153e+07 -7.141e+07 -7.128e+07 -7.115e+07 -7.102e+07 -7.09e+07 -7.077e+07 -7.064e+07 -7.052e+07 -7.039e+07 -7.026e+07 -7.013e+07 -7.001e+07 -6.988e+07 -6.975e+07 -6.963e+07 -6.95e+07 -6.937e+07 -6.924e+07 -6.912e+07 -6.899e+07 -6.886e+07 -6.874e+07 -6.861e+07 -6.848e+07 -6.835e+07 -6.823e+07 -6.81e+07 -6.797e+07 -6.785e+07 -6.772e+07 -6.759e+07 -6.746e+07 -6.734e+07 -6.721e+07 -6.708e+07 -6.696e+07 -6.683e+07 -6.67e+07 -6.657e+07 -6.645e+07 -6.632e+07 -6.619e+07 -6.607e+07 -6.594e+07 -6.581e+07 -6.568e+07 -6.556e+07 -6.543e+07 -6.53e+07 -6.518e+07 -6.505e+07 -6.492e+07 -6.48e+07 -6.467e+07 -6.454e+07 -6.441e+07 -6.429e+07 -6.416e+07 -6.403e+07 -6.391e+07 -6.378e+07 -6.365e+07 -6.352e+07 -6.34e+07 -6.327e+07 -6.314e+07 -6.302e+07 -6.289e+07;-3.055e+07 -3.03e+07 -3.005e+07 -2.98e+07 -2.955e+07 -2.93e+07 -2.905e+07 -2.88e+07 -2.855e+07 -2.831e+07 -2.806e+07 -2.781e+07 -2.756e+07 -2.731e+07 -2.706e+07 -2.681e+07 -2.656e+07 -2.631e+07 -2.606e+07 -2.581e+07 -2.556e+07 -2.531e+07 -2.506e+07 -2.481e+07 -2.456e+07 -2.431e+07 -2.406e+07 -2.381e+07 -2.357e+07 -2.332e+07 -2.307e+07 -2.282e+07 -2.257e+07 -2.232e+07 -2.207e+07 -2.182e+07 -2.157e+07 -2.132e+07 -2.107e+07 -2.082e+07 -2.057e+07 -2.032e+07 -2.007e+07 -1.982e+07 -1.957e+07 -1.932e+07 -1.907e+07 -1.882e+07 -1.858e+07 -1.833e+07 -1.808e+07 -1.783e+07 -1.758e+07 -1.733e+07 -1.708e+07 -1.683e+07 -1.658e+07 -1.633e+07 -1.608e+07 -1.583e+07 -1.558e+07 -1.533e+07 -1.508e+07 -1.483e+07 -1.458e+07 -1.433e+07 -1.408e+07 -1.384e+07 -1.359e+07 -1.334e+07 -1.309e+07 -1.284e+07 -1.259e+07 -1.234e+07 -1.209e+07 -1.184e+07 -1.159e+07 -1.134e+07 -1.109e+07 -1.084e+07 -1.059e+07 -1.034e+07 -1.009e+07 -9.844e+06 -9.594e+06 -9.345e+06 -9.095e+06 -8.846e+06 -8.596e+06 -8.347e+06 -8.097e+06 -7.848e+06 -7.598e+06 -7.349e+06 -7.099e+06 -6.85e+06 -6.6e+06 -6.351e+06 -6.101e+06 -5.852e+06;2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07]; 
sat_pos_4=[5.573e+07 5.546e+07 5.519e+07 5.492e+07 5.466e+07 5.439e+07 5.412e+07 5.385e+07 5.358e+07 5.331e+07 5.304e+07 5.277e+07 5.25e+07 5.224e+07 5.197e+07 5.17e+07 5.143e+07 5.116e+07 5.089e+07 5.062e+07 5.035e+07 5.008e+07 4.982e+07 4.955e+07 4.928e+07 4.901e+07 4.874e+07 4.847e+07 4.82e+07 4.793e+07 4.766e+07 4.74e+07 4.713e+07 4.686e+07 4.659e+07 4.632e+07 4.605e+07 4.578e+07 4.551e+07 4.524e+07 4.498e+07 4.471e+07 4.444e+07 4.417e+07 4.39e+07 4.363e+07 4.336e+07 4.309e+07 4.282e+07 4.256e+07 4.229e+07 4.202e+07 4.175e+07 4.148e+07 4.121e+07 4.094e+07 4.067e+07 4.04e+07 4.014e+07 3.987e+07 3.96e+07 3.933e+07 3.906e+07 3.879e+07 3.852e+07 3.825e+07 3.798e+07 3.772e+07 3.745e+07 3.718e+07 3.691e+07 3.664e+07 3.637e+07 3.61e+07 3.583e+07 3.556e+07 3.53e+07 3.503e+07 3.476e+07 3.449e+07 3.422e+07 3.395e+07 3.368e+07 3.341e+07 3.315e+07 3.288e+07 3.261e+07 3.234e+07 3.207e+07 3.18e+07 3.153e+07 3.126e+07 3.099e+07 3.073e+07 3.046e+07 3.019e+07 2.992e+07 2.965e+07 2.938e+07 2.911e+07;-4.208e+07 -4.216e+07 -4.223e+07 -4.231e+07 -4.239e+07 -4.247e+07 -4.255e+07 -4.262e+07 -4.27e+07 -4.278e+07 -4.286e+07 -4.294e+07 -4.302e+07 -4.309e+07 -4.317e+07 -4.325e+07 -4.333e+07 -4.341e+07 -4.348e+07 -4.356e+07 -4.364e+07 -4.372e+07 -4.38e+07 -4.387e+07 -4.395e+07 -4.403e+07 -4.411e+07 -4.419e+07 -4.427e+07 -4.434e+07 -4.442e+07 -4.45e+07 -4.458e+07 -4.466e+07 -4.473e+07 -4.481e+07 -4.489e+07 -4.497e+07 -4.505e+07 -4.512e+07 -4.52e+07 -4.528e+07 -4.536e+07 -4.544e+07 -4.552e+07 -4.559e+07 -4.567e+07 -4.575e+07 -4.583e+07 -4.591e+07 -4.598e+07 -4.606e+07 -4.614e+07 -4.622e+07 -4.63e+07 -4.637e+07 -4.645e+07 -4.653e+07 -4.661e+07 -4.669e+07 -4.677e+07 -4.684e+07 -4.692e+07 -4.7e+07 -4.708e+07 -4.716e+07 -4.723e+07 -4.731e+07 -4.739e+07 -4.747e+07 -4.755e+07 -4.762e+07 -4.77e+07 -4.778e+07 -4.786e+07 -4.794e+07 -4.802e+07 -4.809e+07 -4.817e+07 -4.825e+07 -4.833e+07 -4.841e+07 -4.848e+07 -4.856e+07 -4.864e+07 -4.872e+07 -4.88e+07 -4.887e+07 -4.895e+07 -4.903e+07 -4.911e+07 -4.919e+07 -4.926e+07 -4.934e+07 -4.942e+07 -4.95e+07 -4.958e+07 -4.966e+07 -4.973e+07 -4.981e+07;2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07]; 
sat_pos_5=[3.529e+06 3.418e+06 3.306e+06 3.195e+06 3.084e+06 2.973e+06 2.862e+06 2.75e+06 2.639e+06 2.528e+06 2.417e+06 2.306e+06 2.194e+06 2.083e+06 1.972e+06 1.861e+06 1.75e+06 1.638e+06 1.527e+06 1.416e+06 1.305e+06 1.194e+06 1.082e+06 9.712e+05 8.6e+05 7.488e+05 6.376e+05 5.264e+05 4.152e+05 3.04e+05 1.928e+05 8.155e+04 -2.965e+04 -1.408e+05 -2.52e+05 -3.633e+05 -4.745e+05 -5.857e+05 -6.969e+05 -8.081e+05 -9.193e+05 -1.03e+06 -1.142e+06 -1.253e+06 -1.364e+06 -1.475e+06 -1.586e+06 -1.698e+06 -1.809e+06 -1.92e+06 -2.031e+06 -2.142e+06 -2.254e+06 -2.365e+06 -2.476e+06 -2.587e+06 -2.698e+06 -2.81e+06 -2.921e+06 -3.032e+06 -3.143e+06 -3.254e+06 -3.366e+06 -3.477e+06 -3.588e+06 -3.699e+06 -3.81e+06 -3.922e+06 -4.033e+06 -4.144e+06 -4.255e+06 -4.367e+06 -4.478e+06 -4.589e+06 -4.7e+06 -4.811e+06 -4.923e+06 -5.034e+06 -5.145e+06 -5.256e+06 -5.367e+06 -5.479e+06 -5.59e+06 -5.701e+06 -5.812e+06 -5.923e+06 -6.035e+06 -6.146e+06 -6.257e+06 -6.368e+06 -6.479e+06 -6.591e+06 -6.702e+06 -6.813e+06 -6.924e+06 -7.035e+06 -7.147e+06 -7.258e+06 -7.369e+06 -7.48e+06;5.906e+07 5.931e+07 5.957e+07 5.983e+07 6.008e+07 6.034e+07 6.06e+07 6.086e+07 6.111e+07 6.137e+07 6.163e+07 6.188e+07 6.214e+07 6.24e+07 6.265e+07 6.291e+07 6.317e+07 6.343e+07 6.368e+07 6.394e+07 6.42e+07 6.445e+07 6.471e+07 6.497e+07 6.522e+07 6.548e+07 6.574e+07 6.6e+07 6.625e+07 6.651e+07 6.677e+07 6.702e+07 6.728e+07 6.754e+07 6.779e+07 6.805e+07 6.831e+07 6.856e+07 6.882e+07 6.908e+07 6.934e+07 6.959e+07 6.985e+07 7.011e+07 7.036e+07 7.062e+07 7.088e+07 7.113e+07 7.139e+07 7.165e+07 7.191e+07 7.216e+07 7.242e+07 7.268e+07 7.293e+07 7.319e+07 7.345e+07 7.37e+07 7.396e+07 7.422e+07 7.448e+07 7.473e+07 7.499e+07 7.525e+07 7.55e+07 7.576e+07 7.602e+07 7.627e+07 7.653e+07 7.679e+07 7.704e+07 7.73e+07 7.756e+07 7.782e+07 7.807e+07 7.833e+07 7.859e+07 7.884e+07 7.91e+07 7.936e+07 7.961e+07 7.987e+07 8.013e+07 8.039e+07 8.064e+07 8.09e+07 8.116e+07 8.141e+07 8.167e+07 8.193e+07 8.218e+07 8.244e+07 8.27e+07 8.296e+07 8.321e+07 8.347e+07 8.373e+07 8.398e+07 8.424e+07 8.45e+07;2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07 2e+07]; 
rho=[1.809e+04 1.808e+04 1.81e+04 1.809e+04 1.81e+04 1.811e+04 1.814e+04 1.816e+04 1.821e+04 1.823e+04 1.824e+04 1.825e+04 1.826e+04 1.825e+04 1.825e+04 1.827e+04 1.831e+04 1.83e+04 1.829e+04 1.833e+04 1.833e+04 1.833e+04 1.835e+04 1.833e+04 1.834e+04 1.835e+04 1.836e+04 1.84e+04 1.841e+04 1.843e+04 1.845e+04 1.843e+04 1.847e+04 1.847e+04 1.847e+04 1.847e+04 1.849e+04 1.85e+04 1.852e+04 1.851e+04 1.852e+04 1.851e+04 1.852e+04 1.852e+04 1.852e+04 1.852e+04 1.85e+04 1.853e+04 1.852e+04 1.856e+04 1.856e+04 1.857e+04 1.858e+04 1.86e+04 1.86e+04 1.862e+04 1.862e+04 1.861e+04 1.86e+04 1.861e+04 1.86e+04 1.86e+04 1.858e+04 1.86e+04 1.86e+04 1.86e+04 1.86e+04 1.86e+04 1.862e+04 1.861e+04 1.859e+04 1.859e+04 1.858e+04 1.855e+04 1.856e+04 1.855e+04 1.852e+04 1.855e+04 1.853e+04 1.851e+04 1.854e+04 1.85e+04 1.85e+04 1.847e+04 1.848e+04 1.845e+04 1.845e+04 1.845e+04 1.844e+04 1.839e+04 1.841e+04 1.84e+04 1.839e+04 1.839e+04 1.837e+04 1.838e+04 1.835e+04 1.833e+04 1.834e+04 1.834e+04;1.877e+04 1.876e+04 1.875e+04 1.875e+04 1.875e+04 1.873e+04 1.869e+04 1.869e+04 1.865e+04 1.863e+04 1.864e+04 1.861e+04 1.859e+04 1.86e+04 1.859e+04 1.858e+04 1.855e+04 1.857e+04 1.854e+04 1.85e+04 1.851e+04 1.85e+04 1.847e+04 1.848e+04 1.847e+04 1.845e+04 1.844e+04 1.84e+04 1.84e+04 1.838e+04 1.839e+04 1.838e+04 1.836e+04 1.835e+04 1.831e+04 1.83e+04 1.83e+04 1.83e+04 1.829e+04 1.827e+04 1.827e+04 1.829e+04 1.826e+04 1.824e+04 1.825e+04 1.821e+04 1.825e+04 1.823e+04 1.822e+04 1.819e+04 1.818e+04 1.817e+04 1.816e+04 1.814e+04 1.815e+04 1.813e+04 1.814e+04 1.814e+04 1.815e+04 1.817e+04 1.816e+04 1.815e+04 1.812e+04 1.812e+04 1.809e+04 1.81e+04 1.809e+04 1.812e+04 1.811e+04 1.812e+04 1.81e+04 1.814e+04 1.812e+04 1.813e+04 1.812e+04 1.812e+04 1.811e+04 1.812e+04 1.813e+04 1.81e+04 1.813e+04 1.809e+04 1.811e+04 1.813e+04 1.811e+04 1.814e+04 1.815e+04 1.814e+04 1.814e+04 1.816e+04 1.816e+04 1.818e+04 1.82e+04 1.82e+04 1.818e+04 1.821e+04 1.821e+04 1.823e+04 1.824e+04 1.824e+04;1.881e+04 1.883e+04 1.883e+04 1.884e+04 1.884e+04 1.884e+04 1.882e+04 1.881e+04 1.88e+04 1.879e+04 1.878e+04 1.877e+04 1.876e+04 1.876e+04 1.879e+04 1.877e+04 1.875e+04 1.875e+04 1.872e+04 1.874e+04 1.869e+04 1.869e+04 1.871e+04 1.87e+04 1.868e+04 1.869e+04 1.869e+04 1.868e+04 1.867e+04 1.867e+04 1.867e+04 1.867e+04 1.868e+04 1.863e+04 1.863e+04 1.864e+04 1.861e+04 1.859e+04 1.859e+04 1.857e+04 1.858e+04 1.858e+04 1.855e+04 1.855e+04 1.853e+04 1.854e+04 1.855e+04 1.853e+04 1.852e+04 1.85e+04 1.848e+04 1.845e+04 1.847e+04 1.844e+04 1.843e+04 1.841e+04 1.839e+04 1.838e+04 1.841e+04 1.84e+04 1.841e+04 1.841e+04 1.837e+04 1.834e+04 1.836e+04 1.835e+04 1.832e+04 1.832e+04 1.832e+04 1.829e+04 1.83e+04 1.831e+04 1.829e+04 1.829e+04 1.828e+04 1.828e+04 1.825e+04 1.826e+04 1.823e+04 1.823e+04 1.821e+04 1.823e+04 1.821e+04 1.819e+04 1.815e+04 1.817e+04 1.815e+04 1.814e+04 1.813e+04 1.813e+04 1.811e+04 1.812e+04 1.811e+04 1.81e+04 1.811e+04 1.812e+04 1.81e+04 1.809e+04 1.807e+04 1.81e+04;1.83e+04 1.83e+04 1.827e+04 1.828e+04 1.827e+04 1.825e+04 1.824e+04 1.823e+04 1.822e+04 1.823e+04 1.822e+04 1.821e+04 1.821e+04 1.821e+04 1.822e+04 1.82e+04 1.821e+04 1.822e+04 1.82e+04 1.817e+04 1.818e+04 1.819e+04 1.82e+04 1.818e+04 1.817e+04 1.815e+04 1.815e+04 1.813e+04 1.813e+04 1.814e+04 1.813e+04 1.813e+04 1.815e+04 1.813e+04 1.814e+04 1.813e+04 1.813e+04 1.815e+04 1.816e+04 1.816e+04 1.811e+04 1.815e+04 1.813e+04 1.815e+04 1.815e+04 1.814e+04 1.814e+04 1.816e+04 1.814e+04 1.813e+04 1.813e+04 1.814e+04 1.814e+04 1.817e+04 1.818e+04 1.817e+04 1.82e+04 1.82e+04 1.822e+04 1.82e+04 1.821e+04 1.82e+04 1.819e+04 1.82e+04 1.82e+04 1.822e+04 1.82e+04 1.82e+04 1.825e+04 1.824e+04 1.825e+04 1.826e+04 1.826e+04 1.828e+04 1.826e+04 1.827e+04 1.829e+04 1.829e+04 1.83e+04 1.831e+04 1.83e+04 1.83e+04 1.83e+04 1.831e+04 1.831e+04 1.835e+04 1.836e+04 1.837e+04 1.836e+04 1.838e+04 1.84e+04 1.84e+04 1.843e+04 1.842e+04 1.84e+04 1.843e+04 1.843e+04 1.844e+04 1.846e+04 1.843e+04;1.816e+04 1.818e+04 1.818e+04 1.817e+04 1.82e+04 1.821e+04 1.824e+04 1.827e+04 1.829e+04 1.832e+04 1.832e+04 1.834e+04 1.836e+04 1.835e+04 1.835e+04 1.838e+04 1.839e+04 1.839e+04 1.84e+04 1.842e+04 1.843e+04 1.844e+04 1.846e+04 1.847e+04 1.848e+04 1.848e+04 1.85e+04 1.852e+04 1.854e+04 1.854e+04 1.858e+04 1.857e+04 1.856e+04 1.859e+04 1.86e+04 1.86e+04 1.862e+04 1.862e+04 1.862e+04 1.866e+04 1.865e+04 1.865e+04 1.866e+04 1.867e+04 1.866e+04 1.868e+04 1.866e+04 1.866e+04 1.866e+04 1.871e+04 1.872e+04 1.873e+04 1.874e+04 1.872e+04 1.875e+04 1.875e+04 1.877e+04 1.874e+04 1.874e+04 1.873e+04 1.873e+04 1.874e+04 1.875e+04 1.874e+04 1.875e+04 1.875e+04 1.878e+04 1.877e+04 1.876e+04 1.877e+04 1.874e+04 1.875e+04 1.872e+04 1.873e+04 1.874e+04 1.873e+04 1.873e+04 1.872e+04 1.871e+04 1.873e+04 1.873e+04 1.875e+04 1.872e+04 1.87e+04 1.87e+04 1.868e+04 1.867e+04 1.87e+04 1.867e+04 1.866e+04 1.861e+04 1.859e+04 1.859e+04 1.86e+04 1.863e+04 1.86e+04 1.86e+04 1.86e+04 1.855e+04 1.857e+04]; 
A=[0.9995 0.03141 0;-0.03141 0.9995 0;0 0 0.9]; 
X=[342.6 350.3 358 366.4 375.4 375.6 369.7 365.2 358.5 359.5 352.6 352.3 346.2 347.1 354.3 355.2 351.7 338.7 327.9 329 316 305.1 309.9 308.3 325.5 326.2 318.9 313.6 299.9 296.8 314.1 300.6 289.4 267.7 262.1 269.7 257.4 241.6 227.9 217.4 227 230.4 211.3 197.7 184.5 183.3 189.1 166.1 173.1 155.5 141.6 123.4 120.6 85.83 74.33 45.67 21.01 27.17 34.81 33.77 34.25 19.42 1.945 -10.86 -17.98 -31.48 -42.29 -55.63 -80.84 -87.15 -97.57 -86.72 -98.28 -105.4 -115.2 -128.5 -131.1 -145.2 -155.9 -172.3 -180.5 -193.9 -206 -220.6 -229.9 -256.3 -265.1 -283.1 -287.3 -291.4 -291.4 -304.4 -308 -311 -308.6 -316.6 -336.5 -338.6 -335.5 -329.8;251.4 237.4 233 224.8 215.6 198.1 164.2 148.7 123.7 100.2 102.3 82.72 59.6 64.05 58.64 40.66 33.2 25.29 12.53 -17.83 -17.6 -29.33 -41.42 -42 -56.89 -54.95 -78.5 -105.1 -118.9 -125.1 -137 -145.4 -151.6 -166.4 -171.9 -176.8 -196 -191.1 -199.4 -211.6 -230.8 -214.1 -228 -244.5 -234.3 -250.4 -242.9 -246.5 -251.2 -280.3 -292.1 -295 -302.5 -300.3 -318 -324.7 -319.9 -314.1 -307.3 -301.4 -290.2 -306.1 -319.2 -332 -325.5 -336.4 -346.4 -346.4 -343 -331.1 -323 -319 -307.4 -303.1 -315.7 -315.1 -310.5 -306 -300 -306.6 -309.7 -312.2 -304.2 -290.1 -295.3 -275.3 -265.4 -266.4 -270.4 -249.4 -227 -210.9 -202.6 -213.1 -224.5 -204.5 -201.4 -178.5 -176.3 -176.8;41.91 48.67 49.49 58.98 43 43.14 48.69 31.96 26.15 10.34 12.21 15.96 13.17 5.786 3.17 -7.392 0.6298 16.1 26.39 18.82 15.81 29.44 31.29 28.72 26.89 21.45 30.03 16.74 16.8 20.12 4.013 2.221 3.067 0.9024 11.4 3.526 2.696 6.835 -13.94 3.654 1.154 3.331 16.41 12.91 19.87 33.24 20.82 4.996 23.2 12.5 16.33 12.34 4.488 -6.042 -16.7 -21.55 -32.63 -28.45 -24.96 -16.91 -20.65 -19.09 -10.51 -18.34 -15.04 -7.734 0.5477 -12.09 -27.81 -35.77 -18.55 -26.07 -25.41 -23.28 -17.56 -19.63 -2.251 -5.182 -12.66 -0.7379 -8.187 -4.499 5.183 13.14 9.078 2.166 10.77 9.563 12.45 18.91 29.34 26.6 12.64 16.67 17.85 11.31 13.49 26.63 32.08 32.73]; %real trajectory of x to be used only to test your solution