Part5.2D_Kalman_Filter_Example
\(Key:\)
\[\begin{align*}
&z_1 = 6.5 mm,\sigma_1 = 0.2 mm;z_2 = 7.3 mm,\sigma_2 = 0.4 mm & \\
&求最优估计: \hat{z} = ? \\
\hat{z} &= z_1 + \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2} (z_2 - z_1) \\
&= 6.5 + \frac{0.2^2}{0.2^2 + 0.4^2} \cdot (7.3 - 6.5) \\
&= 6.66 \\
So:& \\
&The\ key\ is\ 6.66.
\end{align*}
\]
\(Example:\)
\[\begin{align*}
States&: \\
&x_1:位置;x_2:速度& \\
匀速&: \\
&位置:x_{1,k} = x_{1,k-1} + \Delta T x_{2,k-1} = x_{1,k-1} + x_{2,k-1},{\color{red}{\Delta T = 1}} \\
&速度:x_{2,k} = x_{2,k-1} \\
&采样时间:\Delta T\ k时刻与k-1时刻的间隔 \\
因为&具有不确定性：\\
&位置:x_{1,k} = x_{1,k-1} + x_{2,k-1} + w_{1,k-1} \\
&速度:x_{2,k} = x_{2,k-1} + w_{2,k-1} \\
&w为Process\ Noise(过程噪声),p(w) \sim N(0,Q) \\
测量&: \\
&z_{1,k} = x_{1,k} \\
&z_{2,k} = x_{2,k} \\
同样&因为具有不确定性：\\
&z_{1,k} = x_{1,k} + v_{1,k} \\
&z_{2,k} = x_{2,k} + v_{2,k} \\
&v为Measure\ Noise(过程噪声),p(v) \sim N(0,R) \\
\therefore\ &
\begin{aligned}
&\begin{bmatrix}
x_{1,k} \\
x_{2,k} \\
\end{bmatrix}
=
\begin{bmatrix}
1 & 1 \\
0 & 1 \\
\end{bmatrix}
\begin{bmatrix}
x_{1,k-1} \\
x_{2,k-1} \\
\end{bmatrix}
+
\begin{bmatrix}
w_{1,k-1} \\
w_{2,k-1} \\
\end{bmatrix}
\Rightarrow {\color{red}{X_k = A X_{k-1} + w_{k-1}}} \\
&\begin{bmatrix}
z_{1,k} \\
z_{2,k} \\
\end{bmatrix}
=
\begin{bmatrix}
1 & 0 \\
0 & 1 \\
\end{bmatrix}
\begin{bmatrix}
x_{1,k} \\
x_{2,k} \\
\end{bmatrix}
+
\begin{bmatrix}
v_{1,k} \\
v_{2,k} \\
\end{bmatrix}
\Rightarrow {\color{red}{Z_k = H X_k + v_k}} \\
\end{aligned}
{\color{red}{\Rightarrow \hat{X}_k最优}}
\end{align*}
\]
\(预测\)
\[\begin{align*}
&先验: \hat{X}_k^- = A \hat{X}_{k-1}^- + B u_{k-1}& \\
&先验误差协方差: P_k^- = A P_{k-1} A^T + Q,{\color{green}{P_{k-1} \rightarrow 上一次误差的协方差}} \\
\end{align*}
\]
\(校正\)
\[\begin{align*}
Kalman\ Gain:K_k &=\frac{P_k^- H^T}{H P_k^- H^T + R}& \\
后验估计: \hat{X}_k &= \hat{X}_k^- + K_k (Z_k - H \hat{X}_k^-) \\
更新误差协方差: P_k &= (I - K_k H) P_k^- \\
\end{align*}
\]
参考文献
下面是在评论区中找到的同学分享的代码：
# https://zhuanlan.zhihu.com/p/48876718
# https://www.bilibili.com/video/BV1dV411B7ME?share_source=copy_web
# DR_CAN例子的python实现
import numpy as np
import matplotlib.pyplot as plt
def gaussian_distribution_generator(var):
return np.random.normal(loc=0.0, scale=var, size=None)
# 状态转移矩阵，上一时刻的状态转移到当前时刻
A = np.array([[1, 1],
[0, 1]])
# 过程噪声协方差矩阵Q，p(w)~N(0,Q)，噪声来自真实世界中的不确定性
Q = np.array([[0.1, 0],
[0, 0.1]])
# 观测噪声协方差矩阵R，p(v)~N(0,R)
R = np.array([[1, 0],
[0, 1]])
# 状态观测矩阵
H = np.array([[1, 0],
[0, 1]])
# 控制输入矩阵B
B = None
# 初始位置与速度
X0 = np.array([[0],
[1]])
# 状态估计协方差矩阵P初始化
P = np.array([[1, 0],
[0, 1]])
if __name__ == "__main__":
# ---------------------------初始化-------------------------
X_true = np.array(X0) # 真实状态初始化
X_posterior = np.array(X0)
P_posterior = np.array(P)
speed_true = []
position_true = []
speed_measure = []
position_measure = []
speed_prior_est = []
position_prior_est = []
speed_posterior_est = []
position_posterior_est = []
for i in range(30):
# -----------------------生成真实值----------------------
# 生成过程噪声
w = np.array([[gaussian_distribution_generator(Q[0, 0])],
[gaussian_distribution_generator(Q[1, 1])]])
X_true = np.dot(A, X_true) + w # 得到当前时刻状态
speed_true.append(X_true[1, 0])
position_true.append(X_true[0, 0])
# -----------------------生成观测值----------------------
# 生成观测噪声
v = np.array([[gaussian_distribution_generator(R[0, 0])],
[gaussian_distribution_generator(R[1, 1])]])
Z_measure = np.dot(H, X_true) + v # 生成观测值,H为单位阵E
position_measure.append(Z_measure[0, 0])
speed_measure.append(Z_measure[1, 0])
# ----------------------进行先验估计---------------------
X_prior = np.dot(A, X_posterior)
position_prior_est.append(X_prior[0, 0])
speed_prior_est.append(X_prior[1, 0])
# 计算状态估计协方差矩阵P
P_prior_1 = np.dot(A, P_posterior)
P_prior = np.dot(P_prior_1, A.T) + Q
# ----------------------计算卡尔曼增益,用numpy一步一步计算Prior and posterior
k1 = np.dot(P_prior, H.T)
k2 = np.dot(np.dot(H, P_prior), H.T) + R
K = np.dot(k1, np.linalg.inv(k2))
# ---------------------后验估计------------
X_posterior_1 = Z_measure - np.dot(H, X_prior)
X_posterior = X_prior + np.dot(K, X_posterior_1)
position_posterior_est.append(X_posterior[0, 0])
speed_posterior_est.append(X_posterior[1, 0])
# 更新状态估计协方差矩阵P
P_posterior_1 = np.eye(2) - np.dot(K, H)
P_posterior = np.dot(P_posterior_1, P_prior)
# 可视化显示
if True:
fig, axs = plt.subplots(1,2)
axs[0].plot(speed_true, "-", label="speed_true", linewidth=1) # Plot some data on the axes.
axs[0].plot(speed_measure, "-", label="speed_measure", linewidth=1) # Plot some data on the axes.
axs[0].plot(speed_prior_est, "-", label="speed_prior_est", linewidth=1) # Plot some data on the axes.
axs[0].plot(speed_posterior_est, "-", label="speed_posterior_est", linewidth=1) # Plot some data on the axes.
axs[0].set_title("speed")
axs[0].set_xlabel('k') # Add an x-label to the axes.
axs[0].legend() # Add a legend.
axs[1].plot(position_true, "-", label="position_true", linewidth=1) # Plot some data on the axes.
axs[1].plot(position_measure, "-", label="position_measure", linewidth=1) # Plot some data on the axes.
axs[1].plot(position_prior_est, "-", label="position_prior_est", linewidth=1) # Plot some data on the axes.
axs[1].plot(position_posterior_est, "-", label="position_posterior_est", linewidth=1) # Plot some data on the axes.
axs[1].set_title("position")
axs[1].set_xlabel('k') # Add an x-label to the axes.
axs[1].legend() # Add a legend.
plt.show()
clc
clear all
%% 时间序列设定
delta_t=0.01;
t=0:delta_t:30;
N = length(t);
sz = [2,N];
X = zeros(sz);
Z = zeros(sz);
%% 矩阵的设定
A = [1 delta_t; 0 1];
H =[1 0; 0 1];
w1=1*randn(2,N);
w2=1*randn(2,N);
X(:,1)=[0; 1];
%% 实际信号和测量信号
for i=2:N
X(:,i)=A*X(:,i-1)+w1(:,i-1);
Z(:,i)=H*X(:,i)+w2(:,i);
end
plot(t,X(1,:),'g')
hold on
plot(t,Z(1,:),'r')
%% 噪声
Q=[0.1 0; 0 0.1];
R=[0.1 0; 0 0.1];
n=size(Q);
m=size(R);
%% 观测值数据初始化
xhat=zeros(sz);
xhatminus=zeros(sz);
P=zeros(n);
Pminus=zeros(n);
K=zeros(n(1),m(1));
I=eye(n(1));
xhat(:,1)=[2; 0];
P=eye(n(1));
%% 卡尔曼滤波
for k=2:N
xhatminus(:,k)=A*xhat(:,k-1);
Pminus=A*P*A'+Q;
% 滤波
K = Pminus*H'*inv(H*Pminus*H'+R);
xhat(:,k)=xhatminus(:,k)+K*(Z(:,k)-H*xhatminus(:,k));
P=(I-K*H)*Pminus;
end
figure
plot(t,X(1,:),'g')
hold on
plot(t,Z(1,:),'r')
hold on
plot(t,xhat(1,:),'b','linewidth',0.5)
figure
plot(t,X(2,:),'g')
hold on
plot(t,Z(2,:),'r')
hold on
plot(t,xhat(2,:),'b','linewidth',0.5)
浙公网安备 33010602011771号