卡尔曼滤波一维实例，测量远山的距离_卡尔曼滤波距离预测-CSDN博客

本文链接：https://blog.csdn.net/m0_63169562/article/details/140767366

1.卡尔曼滤波讲解

1.1关于卡尔曼滤波的背景

①.这个世界上所有的测量仪器都是不准确的（观测不准确）

②.人们的"主观认识"也都不会准确（预测不准确）

1.2 因此卡尔曼滤波的预测方程可以写成如下形式：

预测方程： $x_{k}=A*x{_{k-1}}+B*{u_{k-1}}+W{_{k-1}}$

观测方程： $z{_{k}}=H*x{_{k}}+V{_{k}}$

对于一个系统来说，可以将其写成状态空间的形式，

如： $\dot{x}=A*x+B*u$

它的离散形式即为： $x_{k}=A*x{_{k-1}}+B*{u_{k-1}}$

$W{_{k-1}}$ 与 $V{_{k}}$ 为期望为0，（也可以说是均值为0，这两句话是一个意思），方差为1的高斯白噪声。只有这样，才能进行卡尔曼滤波。

1.3 卡尔曼滤波的公式，关系到代码编写

$\hat{x}_{k}^{-}$ （-代表先验， $\hat{}$ 代表估计）

先验估计误差： $e_{k}^{-}=x_{k}-\hat{x}_{k}^{-}$

后验估计误差： $e_{k}=x_{k}-\hat{x}_{k}$

算法第一步：时间更新（预测）

$\hat{x}_{k}^{-}=A*\hat{x}_{k-1}+B*u_{k-1}$

$P_{k}^{-}=A*P{_{k-1}}*A^{T}+Q$

算法第二步：测量更新（校正）

$K_{K}=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}^{-}$

2.一维实际案例（远山估计）

真实距离为100m。

然后测量值中含有噪声，通过卡尔曼滤波进行观测。

附实际代码，保证可以运行，代码中含有注释，可开发性强！

%雷达测量远山距离的一维数据卡尔曼滤波的matlab程序%
%利用KalmanFilter估计观测点距离远山得真实值实例
Z=repelem(100,1000);%创建一个数组，里面有1000个100
noise=sqrt(1)+randn(1,1000); %产生1000的标准正态分数的数，用来模拟噪声或干扰，方差为1的高斯白噪声
Z = Z + noise;  %将噪声加入z中，此时的z即为带有观测噪声的观测值

X = 0; %设置系统状态的初始值，初始值并不是很重要，一般取0，经过几次迭之后就会趋于真实值
P = 1; %设置协方差矩阵，p一般取1，值越小越容易收敛，但一般不取0
A = 1; %状态转移矩阵
Q = 0; %远山的距离不随时间发生变化，所以Q=0，即没有过程噪声
H = 1; %观测矩阵
R = 1; %R即为观测噪声，方差为1，即z中加入的噪声
figure; %出来画布
hold on;%画布保持
for i=1:1000 %迭代1000次
    X_ = A*X;   %远山得距离一直没变，所以此系统此系统中并没有输入量
    P_ = A*P*A'+Q;
    K = P_*H'/(H*P_*H'+R);
    X = X_+K*(Z(i)-H*X_);
    P = (eye(1)-K*H)*P_;   %以上五个公式为卡尔曼滤波的核心公式
    plot(i,X,'r.'); %卡尔曼滤波
    plot(i,100,'m.');%远山的真实距离
    
    hold on
end