Commonly used algorithms for drones - Kalman filter (XII)

sigma

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

5 Kalman filter application

In the robot tracking based on the rangefinder, the sampling period of the laser rangefinder is 100ms. Assuming that the measured value is obtained at time k and combined with the distance prediction value at time k-1 (Equation 1), the measured value at time k can be filtered and estimated according to Equation 4. After the correction, the distance at time k+1 can be predicted, which is equivalent to one sampling period in advance. Therefore, motion control can be performed in advance.

Taking the laser rangefinder on the robot to detect and track dynamic targets as an example, the application of Kalman filtering in filtering and prediction is explained.

Considering the motion state of the robot, the obstacle target state estimation in the robot coordinate system can be carried out in three cases:

1. The robot is stationary and the target is moving;

2. , the robot moves while the target is stationary, which is equivalent to the robot estimating the relative position of the stationary environment or obstacles and targets, and has the effect of correcting the robot's positioning. (Because the relative motion position determined by the odometer when the robot moves will have errors and need to be corrected; in addition, the relative position of the robot and the target in two adjacent motion cycles and laser sampling cycles is only determined by the laser rangefinder, which has errors. The calculation of the robot position combined with the odometer can more accurately estimate the relative position of the two through Kalman filtering.)

3. When both the robot and the target are in motion, predict the relative motion of the moving target relative to the robot, so as to control the robot. Of course, the third case can include the first two. If the calculation of the first two cases can be obtained by changing different parameters in the algorithm of the third case, then a set of methods can be used to predict the relative motion of moving and static targets in a dynamic environment.

Currently, it is believed that the robot is stationary, and the robot's sensor coordinates use the polar coordinate system.

Suppose the state vector of the system

[font=微软雅黑, The state equation of the observation system is (there is no input, so B is 0): X (k) = AX (k 1) + W (k 1).

Since the sampling period t of the laser rangefinder is 100ms and the target tracked by the robot moves at a relatively slow speed, the target is set to move at a uniform speed within the local time period.

[align =left]

t is the sampling period.

The speed of movement along the x direction is variable, and the average speed in the previous sampling period (or the average speed in the previous two periods) can be taken.

Where:

W(k 1) is the process noise, which is a Gaussian white noise with zero mean and covariance Q(k-1). If the process noise W (k 1) is replaced by T(k-1)v(k-1), T(k) is the distribution matrix of the process noise, v(k) is the error vector corresponding to each state variable, and the error of each state variable conforms to the Gaussian distribution with a mean of 0 (a normal distribution with a mean of 0), that is,

Then Q(k) becomes

The measurement equation is: Z(k) = CX (k) +V (k) ，Where：

ρ and θ are the target radial distance and azimuth measurement data of the laser radar in polar coordinates.

In the above process equation, the speed is calculated by the position difference between the previous and next two sampling periods, which is insufficient to consider the state of target acceleration. Because there is still a certain error in the estimated position each time, subtracting the two position estimates may reduce the overall error or amplify the overall error (that is, the error after subtraction), so it may cause oscillation of speed. Even if the target moves at a constant speed, the filter may fluctuate in tracking, reducing the tracking and prediction effect.