Kalman filter principle and practice (python)

Brief description:

Kalman filtering is an algorithm that uses the linear system state equation to optimally estimate the system state through the input and output observation data of the system. Since the observation data includes the influence of noise and interference in the system, the optimal estimation can also be regarded as a filtering process.

Preparation content:

Variance: The mean of the sum of the squares of the differences between each sample and the sample mean, which measures the degree of dispersion of a set of data. Covariance: The expectation of the overall error of two variables, which measures the degree of linear correlation between two variables. When two variables are the same, covariance is variance. If two variables have opposite trends, and one variable is greater than its own expected value, while the other is less than its own expected value, then the covariance between the two variables is negative. The connection or relationship between the two, the greater the relationship, the greater the covariance. Covariance matrix: The covariance of two or two variables in the data set, each element is the covariance noise between the elements of the vector The smaller the covariance matrix, the smaller the noise error and the higher the credibility, on its diagonal The value of is the variance. The smaller the error covariance matrix, the smaller the relationship between process noise and measurement noise. Separate process noise ω and observation noise υ by proportion. If the relationship is smaller, the separation is more accurate. For example, a pile of white sugar and salt, if the two are mixed evenly, it is said that they are closely related, and it will be more difficult to separate them by the method of proportion.
Covariance Learning Links

linear system

Models , Equations of Motion and Observation Equations

Among them, A is the state transition matrix, B is the controllable input gain; W is the noise of the motion model system (such as the measurement noise of the odometer of the navigation system), and V is the noise of the observation system (such as the camera observation noise of the navigation system);
where These two parts of noise conform to Gaussian distribution:

prediction and update

Note: xk represents the state at time K, xk^hat represents the optimized prediction state at time K (not the real state); and xk- represents the state of prediction at time K, the state (first test) is obtained based on the test result of the previous state, and then corrected to remove - in the update part. Where p- represents the covariance matrix (error matrix). When the estimated state is 1-dimensional data, if noise is added, only the variance sita can be considered; when the estimated state is high-dimensional, in order to consider the degree of deviation of each dimension from the mean, it must be Introduce the covariance matrix sigma . Kk is Kalman gain (observation weight); H is observation noise; Q predicts noise covariance; R observes noise covariance.

The formula derivation is not displayed here, if you are interested, please refer to the information at the bottom of the article (free)

Nonlinear system:

Model , Motion Model and Observation Equations

Linearized (Taylor Series) Predicted and updated
by Taylor's first order expansion at x0

:

overall process

practical part

code：

import numpy as np
import matplotlib.pyplot as plt

delta_t = 0.1                               # 每秒钟采一次样
end_t = 7                                   # 时间长度
time_t = end_t * 10                         # 采样次数
t = np.arange(0, end_t, delta_t)            # 设置时间数组
u = 1                                       # 定义外界对系统的作用 加速度
x = 1 / 2 * u * t ** 2                      # 实际真实位置

v_var = 1                                   # 测量噪声的方差
# 创建高斯噪声，精确到小数点后两位
v_noise = np.round(np.random.normal(0, v_var, time_t), 2)

X = np.mat([[0], [0]])                      # 定义预测优化值的初始状态
v = np.mat(v_noise)                         # 定义测量噪声
z = x + v                                   # 定义测量值（假设测量值=实际状态值+噪声）
A = np.mat([[1, delta_t], [0, 1]])          # 定义状态转移矩阵
B = [[1 / 2 * (delta_t ** 2)], [delta_t]]   # 定义输入控制矩阵
P = np.mat([[1, 0], [0, 1]])                # 定义初始状态协方差矩阵
Q = np.mat([[0.001, 0], [0, 0.001]])        # 定义状态转移(预测噪声)协方差矩阵
H = np.mat([1, 0])                          # 定义观测矩阵
R = np.mat([1])                             # 定义观测噪声协方差
X_mat = np.zeros(time_t)                    # 初始化记录系统预测优化值的列表

for i in range(time_t):
    # 预测
    X_predict = A * X + np.dot(B, u)        # 估算状态变量
    P_predict = A * P * A.T + Q             # 估算状态误差协方差
    # 校正
    K = P_predict * H.T / (H * P_predict * H.T + R)     # 更新卡尔曼增益
    X = X_predict + K * (z[0, i] - H * X_predict)       # 更新预测优化值
    P = (np.eye(2) - K * H) * P_predict                 # 更新状态误差协方差
    # 记录系统的预测优化值
    X_mat[i] = X[0, 0]

plt.rcParams['font.sans-serif'] = ['SimHei']    # 设置正常显示中文
plt.plot(x, "b", label='实际状态值')             # 设置曲线数值
plt.plot(X_mat, "g", label='预测优化值')
plt.plot(z.T, "r--", label='测量值')
plt.xlabel("时间")                               # 设置X轴的名字
plt.ylabel("位移")                               # 设置Y轴的名字
plt.title("卡尔曼滤波示意图")                     # 设置标题
plt.legend()                                    # 设置图例
plt.show()                                      # 显示图表

The notes are detailed, no explanation is given, compare the above five formulas.
Running results:

theoretical derivation and python application can be referred to (more professional resources):
https://download.csdn.net/download/weixin_45080292/85418585
Excellent blog recommendation:
https://medium.com/@jaems33/understanding-kalman- filters-with-python-2310e87b8f48