Commonly used algorithms for drones - Kalman filter (IV)

sigma

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

2.2 Basic formula of Kalman filter Suppose the state equation of the system is: X(i) (k(i)) (i) = AX(i) ((i)  1) AX(i) = AX(i) (k(i)  1) + BU (k 1) +W (k 1) （1）

Measurement equation of the system:

Z (k) = CX (k) + V (k) (2)

Where: A(k-1) is the state transfer matrix; X(k-1) is the state vector; B(k-1) is the input control term matrix; u(k-1) is the input or control signal; W(k-1) is the zero mean, white Gaussian process noise sequence with covariance Q(k-1); C(k) is the measurement matrix; V(k) is the zero mean, white Gaussian measurement noise with covariance R(k). The output term Z(k) of the system's measurement equation is a quantity that can be actually measured.

represents the optimal linear filter estimate of the random signal X(k) at time k, and

represents the optimal linear prediction estimate of the signal X(k+1) at time k for the signal at time k+1.

The recursive algorithm of the Kalman filter is as follows:

The update equation of the covariance has the following different forms:

P(k | k) = P(k | k 1) K (k)C(k)P[/ size](k | k 1) (e1)

[ font=Microsoft Yahei, "]Formula (e) is the same as (e1) and is applicable when the gain is the optimal Kalman gain. If other gains are used, use formula (e3).

(e2)

Among them, the predicted covariance of the measurement (or innovation covariance) is used to measure the uncertainty of the innovation (the measurement value minus the measurement estimate). The smaller the covariance of the innovation, the more accurate the measurement. I don’t know what the application conditions of formula (e2) are, but it should be more adaptable than formula (e1). The book uses this formula for Kalman filter iteration.

(e3)

Where I is the unit matrix of the same dimension as the covariance matrix. Formula (e3) can ensure that the covariance matrix P Symmetry and positive definiteness. This formula is valid for any Kalman gain K.

Where I is the unit matrix of the same dimension as the covariance matrix. Formula (e3) can ensure the symmetry and positive definiteness of the covariance matrix P. This formula is valid for any Kalman gain K.

Formula (e) or (e1) is relatively simple to calculate, so this formula is always used in practice, but it should be noted that this formula is only valid when the optimal Kalman gain is used. If the arithmetic precision is always low and numerical stability problems occur, or if a non-optimal Kalman gain is deliberately used, formula (e3) must be used. However, it is not clear what the optimal Kalman gain is, so you can try a simple form first.

The five equations (a) to (e) above are the iterative formulas of Kalman filtering. If the system is time-invariant, the coefficient matrices A, B, and C are all constant matrices. According to the five formulas above, the entire process of Kalman filtering can be completed and continued. Because the predicted value calculated according to the state equation is combined with the actual measurement value Z(k) at time k to estimate k The state true value X(k|k) at the moment, so it plays the best estimation effect of reducing the error and realizes the correction of the measured value.

Process (state) covariance matrix P

The standard covariance formula of two scalars x1 and x2 is cov(x1 , x2 ) = E{[x1  E(x1 )][x2 E(x2 )]} .

Assume that the state vector X has q Components, the errors of each state component after one-step filtering:

The mean square error P(k) of the error becomes a covariance matrix of the error

[i mg]https://bbs.nuedc-training.com.cn/data/attachment/forum/201811/19/173220j69kejszk6h62sjg.png.thumb.jpg[/img]

where Pij(k) is the k moment state component xi and x[/size ]j Covariance of the filtering error: https://bbs.nuedc-training.com.cn/data/attachment/forum/201811/19/173220k397r856tqq7bqq8.png.thumb.jpgnuedc-training.com.cn/data/attachment/forum/201811/19/173220qtit888z18l684tp.png.thumb.jpg[/img]

The mean square error P(k) of the error becomes a covariance matrix of the error

where Pij(k) is the k moment state component xi and x[/size ]j Covariance of the filtering error: https://bbs.nuedc-training.com.cn/data/attachment/forum/201811/19/173220k397r856tqq7bqq8.png.thumb.jpgnuedc-training.com.cn/data/attachment/forum/201811/19/173220qtit888z18l684tp.png.thumb.jpg[/img]

The mean square error P(k) of the error becomes a covariance matrix of the error

where Pij(k) is the k moment state component xi and x[/size ]j Covariance of the filtering error: https://bbs.nuedc-training.com.cn/data/attachment/forum/201811/19/173220k397r856tqq7bqq8.png.thumb.jpg

月下良缘

Thanks for sharing!