Commonly used algorithms for drones - Kalman filter (VI)

sigma

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

2.4 Kalman filter initialization Initialization is also very important. Large initialization deviation may lead to vibration or even failure of convergence (?).

For example, when tracking a car, the state space is

https://bbs.nuedc-training.com.cn/data/attachment/forum/201811/20/164537o8m85ls692ml3333.png.thumb.jpg[/img]

If you know the initial position of the car accurately enough, you can initialize

[font=微软雅黑, Or other specific values. And, by assigning a smaller value to the state covariance matrix, we tell the filter that we know the exact initial position, for example, by initializing it to

[font=微软雅黑, Or give other smaller values.

If we do not know the initial position and velocity exactly, then the covariance matrix can be initialized as a matrix whose diagonal elements are b, where b is a suitably large number.

At this time, the filter is more inclined to use the information of the initial measurement value.

1. Two-dimensional state vector estimation

Assume that when the radar performs one-dimensional tracking, the state equation is

Measurement noise V(k) ~ N(0,r) (i.e. V It conforms to the normal distribution with mean 0 and variance r), and is independent of process noise. At this time, the state estimation can be initialized by using the initial two-point difference, using the two measured values Z(0) and Z(1) at the first and second moments.

The initial covariance is:

T is the sampling period of measurement. State estimation and filtering start from time k=2.

2 Initialization of four-dimensional state vector estimation

Belonging to the data processing of two-coordinate radar, the state vector of the system is expressed as

The state equation is: X(k) = AX(k1)+W(k1), the measurement equation is: Z(k)= CX(k) +V(k), the measurement state vector is:

[font=微软雅黑, （a）

Note that the rectangular coordinate system is selected here to represent the target state data, but the radar measurement data is expressed in polar coordinates. Here, the polar coordinate data is converted into rectangular coordinate data in the measurement variable. The purpose of doing this may be to turn the measurement equation into a linear equation. In this case, the ordinary Kalman filter method can be used. Otherwise, the measurement vector is expressed in polar coordinates and written as

(b)

is a nonlinear equation and can only be written in the form of Z(k)=f(X(k))+V(k), which requires the use of the extended Kalman filter method.

The initial state of the system can be determined by the measured values z(0) and z(1) at the first two moments, that is,

（c）

Since the initial state value is given by the measured value, the initial state covariance matrix P is also set accordingly by the measured variance. Since the measured quantity (i.e., output quantity) in the measurement equation is converted from the real measured values ρ and θ, the measurement noise of measuring Z also needs to be calculated from the measurement noise of the direct measured values ρ and θ.

The covariance of the measurement noise at time k in the rectangular coordinate system is:

Of course, if the output of the measurement equation is directly ρ and θ , the measurement vector is in the form of equation (b), and the state equation remains unchanged, then the initial state covariance matrix may still need to be written in the form of (f), because the state quantity is still converted from the measurement quantity, and the relationship between them has not changed.

However, if the state equation changes, directly use ρ and θ and their speeds as state vectors, and the measurement equation is also output in ρ and θ:

[font=微软雅黑, The state equation is: X (k) = AX (k1) +W (k1) , and the measurement equation is: Z(k) = CX (k) +V (k) The measurement noise V(k) has zero mean and covariance R(k).

Then

月下良缘

Thanks for sharing!