Covariance update

The Kalman gain is then computed in the Equation 4. The measurement update equations are then used to estimate the state and the covariance updates, according to the Equations 5 and 6, respectively ... 

Error covariance update with online measurements... 

Due to the nonlinearity of the mapping, linearization is needed and the Jacobian of mapping and are used in Equations (14) and (16). When a robot detects another vehicle, the covariance update is calculated according to Equation (15), exploiting RP. 

One expects that during the measurement-prediction cycle the confidence in the parameters improves. Thus, the variance-covariance matrix needs also to be updated in each measurement-prediction cycle. This is done as follows [1] ... 

Step 2. Update of the Kalman gain vector k(l) and variance-covariance matrix P(l)... 

Equations (41.15) and (41.19) for the extrapolation and update of system states form the so-called state-space model. The solution of the state-space model has been derived by Kalman and is known as the Kalman filter. Assumptions are that the measurement noise v(j) and the system noise w(/) are random and independent, normally distributed, white and uncorrelated. This leads to the general formulation of a Kalman filter given in Table 41.10. Equations (41.15) and (41.19) account for the time dependence of the system. Eq. (41.15) is the system equation which tells us how the system behaves in time (here in j units). Equation (41.16) expresses how the uncertainty in the system state grows as a function of time (here in j units) if no observations would be made. Q(j - 1) is the variance-covariance matrix of the system noise which contains the variance of w. 

The updated quantities 0 and P represent our best estimates of the unknown parameters and their covariance matrix with information up to and including time tn. Matrix Pn represents an estimate of the parameter covariance matrix since,... 

Suppose that at a time tk, the updated values of the mean and the estimate error covariance (x(t 11 tk- ) and (f 11 i)) are already available where the argument means at time tk-, given information up to time f j. These values are then used... 

The solution of the minimization problem again simplifies to updating steps of a static Kalman filter. For the linear case, matrices A and C do not depend on x and the covariance matrix of error can be calculated in advance, without having actual measurements. When the problem is nonlinear, these matrices depend on the last available estimate of the state vector, and we have the extended Kalman filter. 

Equation (6) updates the current state vector by adding the correction factor, which is givCT by the difference between the actual measurranent z(k) and the predicted state vector, weighted by the Kalman gain vector. Similarly, the Kalman gain is used to estimate the updated current error covariance matrix, as in Eqn.(7). 

This optimization problem can be solved by the MATLAB function fminsearch [171]. It has been shown numerically for the globally identifiable case with a large number of data points that the updated PDF can be well approximated by a Gaussian distribution 0(9 9, H(9 ) ) with mean 9 and covariance matrix H(9 )- -, where U(9 ) denotes the Hessian oiJ(9) calculated ate = 9 ... 

In the case where a non-informative prior is used, the first term can be simply neglected. Furthermore, the updated PDF of the parameter vector 0 can be well approximated by a Gaussian distribution G(0 0, Ti(0 ) ) with mean 0 and covariance matrix H(0 ), where H 0 ) denotes the Hessian matrix of the objective function J calculated at = ... 

The Bayesian fast Fourier transform approach uses the statistical properties of discrete Fourier transforms, instead of the spectral density estimators, to construct the likelihood function and the updated PDF of the model parameters [292]. It does not rely on the approximation of the Wishart distributed spectrum. Expressions of the covariance matrix of the real and imaginary parts of the discrete Fourier transform were given. The only approximation was made on the independency of the discrete Fourier transforms at different frequencies. Therefore, the Bayesian fast Fourier transform approach is more accurate than the spectral density approach in the sense that one of the two approximations in the latter is released. However, since the fast Fourier transform approach considers the real and imaginary parts of the discrete Fourier transform, the corresponding covariance matrices are 2No x 2Nq, instead of No x No in the spectral density approach. Therefore, the spectral density approach is computationally more efficient than the fast Fourier transform approach. 

---