定位算法——Kalman滤波简要入门(理论+实例+matlab实现)-Toy模板网

这篇具有很好参考价值的文章主要介绍了定位算法——Kalman滤波简要入门(理论+实例+matlab实现)。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

Kalman Filter简单介绍

卡尔曼滤波是一种用于估计含有不确定因素的动态系统状态的优化算法，其最初由Rudolf E. Kálmán于1960年代提出。该算法广泛应用于各种工程和科学领域，特别是在控制系统、导航、自动驾驶、信号处理等方面。

卡尔曼滤波是基于概率推理的方法，它通过融合系统的预测模型和测量数据来估计系统的状态，尤其适用于带有噪声的动态系统。在每个时间步骤中，卡尔曼滤波会做出两个主要步骤：

预测步骤（预测阶段）： 根据系统的动态模型和前一个状态的估计，预测当前时刻的状态。这个预测考虑了系统的物理规律以及外部输入。
更新步骤（更新阶段）： 在收到测量数据后，卡尔曼滤波会结合预测的状态和实际测量值，通过加权平均的方式来修正状态估计，从而获得更准确的状态估计值。卡尔曼滤波会根据测量数据的可靠性自动调整权重，对于更可信的数据，其影响会更大。

想象你在驾驶一辆车，你有车速表可以告诉你当前的速度，但是它可能有些抖动或者不准确。同时，你也有眼睛可以看到路上的实际情况，比如你正朝着一个目标行驶。现在，卡尔曼滤波就像一个聪明的司机，他会结合车速表的估计和你眼睛看到的实际情况，以一种明智的方式修正你的速度估计。这样，你的驾驶速度就更准确了。当然卡尔曼滤波的作用可不限于估计车速，它在现代科学和工程领域有着举足轻重的作用。导航系统中飞行器的位置和姿态估计、雷达跟踪目标、视频目标跟踪、信号去噪、物体定位中卡尔曼滤波都能发挥作用。你甚至能实时得到女朋友的最优重量估计(如果有的话)。

引入预测公式

在一维空间中有辆小车向前移动，加速度为 $\frac{f_t}{m}$ ，t时刻瞬时速度为 $v_t$ ，t时刻小车位置为 $x_t$ 。按照初中知识，小车的位置公式如下：
$x_t = x_{t-1} + v_t·\triangle t + \frac{1}{2}\frac{f_{t-1}}{m}\triangle t^2$
小车的速度公式如下：
$v_t = v_{t-1} + \frac{f_{t-1}}{m}\triangle t$

现在假设小车状态用 $X_t=[x_t,v_t]^T$ 表示，上面两个公式可用如下矩阵表示
$X_t = \begin{bmatrix} x_{t} \\ v_{t}\end{bmatrix}= \begin{bmatrix} 1 & \triangle t \\ 0 & 1 \end{bmatrix} * \begin{bmatrix} x_{t-1} \\ v_{t-1} \end{bmatrix} + \begin{bmatrix} \frac{1}{2}\triangle t^2 \\ \triangle t \end{bmatrix}·\frac{f_{t-1}}{m}$

$\begin{bmatrix} 1 & \triangle t \\ 0 & 1 \end{bmatrix}, B=\begin{bmatrix} \frac{1}{2}\triangle t^2 \\ \triangle t \end{bmatrix} ,U_t = \frac{f_{t-1}}{m}$

则上述矩阵表达转化为：
$X_t = F*X_{t-1}+BU_{t-1}$
这里面的 $F$ 便是状态转移矩阵。但是这个预测方程难免受到误差的影响（例如路面不平），所以此时的 $X_t$ 不是最优估计值，因此改为 $\hat{X}^-$ ，其中-号表示先验，并没有加入动态测量的数据，因此并不准确。
据此，我们可以引入卡尔曼滤波的五大核心公式。

Kalman Filter五大核心公式

预测部分：
$\hat{X}^-_t = F \hat{X}_{t-1} + BU_{t-1}$
$P_t^- = FP_{t-1}F^T+Q$
更新部分：
$K_t = P^-_tH^T(HP^-_tH^T+R)^{-1}$
$\hat{X} = X^-_t+K_t(Z_t-H\hat{X}_t^-)$
$P_T=(I-K_tH)P^-_t$

现在解释下各个参数的意义——

$\hat{X}^-_t$ ：t时刻的先验预测状态，并不准确。

$P_T^-$ :预测误差的协方差矩阵，表示t时刻系统的不确定性。

$F$ ：状态转移矩阵，有的系统不像小车运行那样能归纳出F，如测量温度(见matlab实现)

$U_t$ ：t时刻外部对系统的已知影响

$Q$ ：过程噪声的协方差矩阵

$R$ ：观测噪声的协方差矩阵

$K_t$ ：卡尔曼增益，权衡预测状态协方差P和观测状态协方差R的大小来决定二者的重要程度，如果更相信预测模型，则 $K_t$ 变小，如果更相信观测模型，则 $K_t$ 变大。另外，卡尔曼增益包含了所有的预测变量，所以在更新时能够同时更新所有的变量。

$Z_t$ ：实际观测值，有如下公式： $Z_t=HX_t+V$ ，其中 $H$ 是观测矩阵， $V$ 是观测噪声。这个公式是多传感器融合的核心公式，也叫做观测方程。当只观测位置时，则 $H = [1, 0]$ ，得到的 $Z_t$ 是一个数值。

一定要注意观察迭代公式以及里面带有负号上标的符号

matlab实现1:小车运动轨迹

%卡尔曼滤波估计小车的运动，这是一个含有不确定性噪声的动态系统，需要做最优估计
clc;
clear;
Z = (1:100);          %假设小车理想情况下是匀速运动，每隔1s观测一次。后面会加上噪声
noise = randn(1,100); 
Z = Z + 20*noise;        %观测系统的小车的状态，可以适当突出噪声演示kalman的作用

X = [0; 0];           %预测的初始值，设为多少不重要

P = [1 0; 0 1];       %预测系统的初始协方差，设为多少不重要

F = [1 1; 0 1];       %状态转移矩阵，由于1s观测一次，delta_t为1

Q = [0.0001 0; 0 0.0001];   %相信观测转移矩阵，误差设置的较小

H = [1 0];                  %观测矩阵，只观测到小车位置

R = 1;                      %由于noise设置为标准正态分布，这里观测的协方差设置为1

X_data = zeros(500, 2);     %存储小车最优状态估计，初始设置为0，后面会不断更新，500行数据需要迭代500次完成更新

for t=1:500

    X_ = F * X;

    P_ = F * P * F' + Q;

    K = P_ * H' / (H * P_ * H' + R);

    X = X_ + K * (Z(t) - H * X_);

    X_data(t, 1) = X(1);

    X_data(t, 2) = X(2);

    P = (eye(2) - K * H) * P_;

end

 
plot(X_data(:, 1), 'b-');

xlabel('时间/s')

ylabel('小车位置/m')

title('kalman filter');

 

hold on;

plot(Z, 'k+');

legend('kalman最优估计值', '带噪声的观测值');

hold off;

由于kalman是个动态最优估计算法，所以不是最终输出一个最优值，而是每次增加一次观测后都进行后验估计，可以把它们打印出来进行观察。

卡尔曼滤波matlab,高精度定位,matlab,开发语言
关于小车位置的估计，可以看到最优估计是在分布的最中央，这也是我们期望的。

matlab实现2:最优温度估计

clc;
clear;

Z = 24 + randn(1, 4000);     %假设测量值是24加上高斯噪声N(0,1)
X = zeros(1, 4000);          %最优估的初始值，后面会迭代更新

P = zeros(1, 4000);          %预测模型的协方差，注意这里并不是一个对称矩阵
Q = 0.001;                   %相信观测转移矩阵，误差设置的较小
R = 1;                       %观测模型的协方差

for t=2:4000

    X_(t) = X(t-1);    

    P_(t) = P(t-1) + Q;

    K(t) = P_(t) / (P(t) + R);

    X(t) = X_(t) + K(t) * (Z(t) - X_(t));

    P(t) = (1 - K(t)) * P_(t);

end

 

figure;

plot(Z, 'k+');

hold on;

plot(X, 'b-', 'linewidth', 1);

hold on;

plot(24*ones(1, 4000), 'r-', 'linewidth', 1);