正文

本文已参与「新人创作礼」活动，一起开启掘金创作之路。卡尔曼滤波（Kalman Filter）是一种利用线性系统状态方程，利用对系统的观测数据，对系统状态进行最优估计的算法。由于观测数据受到系统中的噪声和干扰的影响，所以系统状态的估计过程也可看作是滤波过程。应用场景之一有利用传感器跟踪感兴趣目标的位置，传感器获取的目标距离、速度、方位角等观测值往往含有噪声。卡尔曼滤波利用目标的动态信息与观测结果相结合，抑制噪声的影响，从而获得一个关于目标位置更准确的估计，这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。

卡尔曼滤波算法matlab

% 利用KalmanFilter估计匀速运动小车的运动状态真实值实例

Z = (1:100);  % 设置一个观察值,表示小车的运动距离从1-100
noise = randn(1, 100);
Z = Z + noise;  % 加入一些噪声,

X = [0; 0];  % 设置系统状态的初始值
P = [1 0; 0 1];  % 设置状态转移矩阵的初值
A = [1 1; 0 1];  % t为采样频率,设置为1,表示每秒采样一次
Q = [0.0001 0; 0 0.0001];  % 将Q的方差设置的很小,就代表了过程噪声很小,对计算出来的估计值比较相信
H = [1 0];  % 这个为什么是1和0解释过了
R = 1;  % 这是观察噪声的协方差矩阵,因为观察噪声基本不会改变,所以设置为一个常数就行
figure;
hold on;
for i = 1:100  % 迭代100次
    X_ = A*X;  % 没有B和ut-1 暂时不考虑小车的加速度情况
    P_ = A*P*A' + Q;
    K = P_*H'/(H*P_*H'+R);
    X = X_+K*(Z(i)-H*X_);
    P = (eye(2)-K*H)*P_;
    plot(X(1), X(2), 'r.'); % 画出小车运动的两个状态,估计出小车的最优真实估计状态解
    plot(X_(1), X_(2), 'b.'); % 画出小车运动的两个状态,估计出小车的最优预测状态解
    hold on;
end