Commonly used algorithms for drones - Kalman filter (VII)

sigma

Commonly used algorithms for drones - Kalman filter (VII) [Copy link]

3. Extended Kalman Filter

Kalman filter is to obtain the dynamic estimation of the target by using the minimum mean square error criterion under the linear Gaussian condition. It is suitable for systems where both the process and the measurement belong to linear systems and the error conforms to the Gaussian distribution. However, in fact, many systems have certain nonlinearity, which is manifested in the nonlinearity of the process equation (state equation) or the nonlinearity of the relationship between the observation and the state (measurement equation). In this case, the general Kalman filter cannot be used. The solution is to linearly approximate the nonlinear relationship and then transform it into a linear problem. The most commonly used linearization method is Taylor series expansion, ignoring higher-order terms. The Kalman filter based on this is the extended Kalman filter (EKF). Taylor expansion can generally take first-order terms or second-order terms (ignoring higher-order terms after the third order). The simulation results show that the performance of the second-order extended Kalman filter is much better than that of the first-order, while the performance of the extended Kalman filter above the second order is not significantly improved compared with the second order, so it is generally not used. The second order has a large amount of calculation, so the first-order Kalman filter is generally used.

Since the state equation or measurement equation may be nonlinear, it cannot be written in a standard linearized format, so it is recorded in the following expression. Of course, for the actual system, it is necessary to write the real state equation and measurement equation expressions, and there is no need to organize them into a standard linearized format.

The following is the recursive formula of the Kalman filter linearized by the first-order Taylor expansion.

State equation: (written directly in the form of a function)

x(k) = f (x(k 1),u(k 1),w(k 1))

Measurement equation:

z(k) = h(x(k),v(k))

3.1 The formula of extended Kalman filter is

[font=微软雅黑, Q(k 1) is the covariance of the noise w(k 1),

R(k) is the covariance of the noise v(k).

月下良缘

Thanks for sharing!