一维数据卡尔曼滤波处理及算法

MATLAB代码如下

clc
%参数初始化
x_hat_0 = 40;       %设置初始估计值为40
e_EST_0 = 5;        %设置初始估计误差为5
e_MEA = 3;          %假设测量误差总为3
%随机生成一组测量值
z_k = 50+(2*rand(1,1000)-1);  
L = length(z_k);
%定义估计值
x_hat = zeros(L,1);
x_hat(1) = x_hat_0;
%定义估计误差
e_EST= zeros(L,1);
e_EST(1) = e_EST_0;
%定义卡尔曼增益
K_k = zeros(L,1);
K_k(1) = 0;

%% 卡尔曼滤波器算法
for k = 2:L+1
    %计算卡尔曼增益
    K_k(k) = e_EST(k-1)/(e_EST(k-1)+e_MEA);
    %计算估计值
    x_hat(k) = x_hat(k-1)+K_k(k)*(z_k(k-1)-x_hat(k-1));
    %更新估计误差
    e_EST(k) = (1-K_k(k))*e_EST(k-1);
end

%% 绘图
figure(1)
plot(z_k)
hold on
plot(x_hat,'LineWidth',2)
axis([0 L+10 35 55])

运行结果如下图所示：

从上图可以看出估计值随着时间能够较好的收敛于准确值。

更多信息可以关注公众号【Future Car】