各种滤波器的实现方式

寻你远山也踏破

这两个卡尔曼滤波器的实现能够让你对预测与校正有个深入理解。
线性卡尔曼滤波器：
Z=(1:2:200);
%观测值汽车的位置也就是我们要修改的量
noise=randn(1,100);
%方差为1的高斯噪声
Z=Z+noise;
X=[0;0];
%初始状态
P=[1 0;0 1];
%状态协方差矩阵
F=[1 1;0 1];
%状态转移矩阵
Q=[0.0001,0;0,0.0001];
%状态转移协方差矩阵
H=[1,0];
%观测矩阵
R=1;
%观测噪声方差
figure;
hold on;
for i =1:100
    %基于上一状态预测当前状态
    X_ =F*X;
    %更新协方差Q系统过程的协方差这两个公式是对系统的预测
    P_ =F*P*F'+Q;
    % 计算卡尔曼增益
    K = P_*H'/(H*P_*H'+R);
    % 得到当前状态的最优化估算值 增益乘以残差
    X = X_+K*(Z(i)-H*X_);
%更新K状态的协方差
P = (eye(2)-K*H)*P_;
scatter(X(1), X(2),4);
%画点，横轴表示位置，纵轴表示速度
end


非线性卡尔曼滤波器：
close all;
clear all;
%%  真实轨迹模拟
kx = .01;   ky = .05;       % 阻尼系数
g = 9.8;                    % 重力
t = 15;                     % 仿真时间
Ts = 0.1;                   % 采样周期
len = fix(t/Ts);            % 仿真步数
dax = 3; day = 3;       % 系统噪声
X = zeros(len,4);
X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值
for k=2:len
    x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
    x = x + vx*Ts;
    vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
    y = y + vy*Ts;
    vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
    X(k,:) = [x, vx, y, vy];
end
%%  构造量测量
dr = 8;  dafa = 0.1;        % 量测噪声
for k=1:len
    r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
    a = atan(X(k,1)/X(k,3))*57.3 + dafa*randn(1,1);
    Z(k,:) = [r, a];
end
%% ekf 滤波
Qk = diag([0; dax/10; 0; day/10])^2;
Rk = diag([dr; dafa])^2;
Pk = 10*eye(4);
Pkk_1 = 10*eye(4);
x_hat = [0,40,400,0]';
X_est = zeros(len,4);
x_forecast = zeros(4,1);
z = zeros(4,1);
for k=1:len
    % 1 状态预测   
    x1 = x_hat(1) + x_hat(2)*Ts;
    vx1 = x_hat(2) + (-kx*x_hat(2)^2)*Ts;
    y1 = x_hat(3) + x_hat(4)*Ts;
    vy1 = x_hat(4) + (ky*x_hat(4)^2-g)*Ts;
    x_forecast = [x1; vx1; y1; vy1];        %预测值
    % 2  观测预测
    r = sqrt(x1*x1+y1*y1);
    alpha = atan(x1/y1)*57.3;
    y_yuce = [r,alpha]';
    %  状态矩阵
    vx = x_forecast(2);  vy = x_forecast(4);
    F = zeros(4,4);
    F(1,1) = 1;  F(1,2) = Ts;
    F(2,2) = 1-2*kx*vx*Ts;
    F(3,3) = 1;  F(3,4) = Ts;
    F(4,4) = 1+2*ky*vy*Ts;
    Pkk_1 = F*Pk*F'+Qk;
    % 观测矩阵
    x = x_forecast(1); y = x_forecast(3);
    H = zeros(2,4);
    r = sqrt(x^2+y^2);  xy2 = 1+(x/y)^2;
    H(1,1) = x/r;  H(1,3) = y/r;
    H(2,1) = (1/y)/xy2;  H(2,3) = (-x/y^2)/xy2;
   
    Kk = Pkk_1*H'*(H*Pkk_1*H'+Rk)^-1;       %计算增益
    x_hat = x_forecast+Kk*(Z(k,:)'-y_yuce);      %校正
    Pk = (eye(4)-Kk*H)*Pkk_1;
    X_est(k,:) = x_hat;
end
%%
figure, hold on, grid on;
plot(X(:,1),X(:,3),'-b');
plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180));
plot(X_est(:,1),X_est(:,3), 'r');
xlabel('X');
ylabel('Y');
title('EKF simulation');
legend('real', 'measurement', 'ekf estimated');
axis([-5,230,290,530]);


粒子滤波器：
clc;
clear all;
close all;
x = 0; %初始值
R = 1;
Q = 1;
tf = 100; %跟踪时长
N = 100; %粒子个数
P = 2;
xhatPart = x;
for i = 1 : N  
  xpart(i) = x + sqrt(P) * randn;%初始状态服从0均值，方差为sqrt(P)的高斯分布
end
xArr = [x];
yArr = [x^2 / 20 + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];
for k = 1 : tf  
    x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;
  %k时刻真实值
  y = x^2 / 20 + sqrt(R) * randn; %k时刻观测值
for i = 1 : N
  xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) ...
    + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;%采样获得N个粒子
  ypart = xpartminus(i)^2 / 20;%每个粒子对应观测值
  vhat = y - ypart;%与真实观测之间的似然
  q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);
  %每个粒子的似然即相似度
end
qsum = sum(q);
for i = 1 : N
  q(i) = q(i) / qsum;%权值归一化
end
for i = 1 : N %根据权值重新采样
   u = rand;
   qtempsum = 0;
   
for j = 1 : N
    qtempsum = qtempsum + q(j);
     if qtempsum >= u
       xpart(i) = xpartminus(j);
       break;
     end
   end
end
xhatPart = mean(xpart);
%最后的状态估计值即为N个粒子的平均值，这里经过重新采样后各个粒子的权值相同
xArr = [xArr x];
yArr = [yArr y];
% xhatArr = [xhatArr xhat];
PArr = [PArr P];
xhatPartArr = [xhatPartArr xhatPart];
end
t = 0 : tf;
figure;
plot(t, xArr, 'b-.', t, xhatPartArr, 'k-');
legend('Real Value','Estimated Value');
set(gca,'FontSize',10);
xlabel('time step');
ylabel('state');
title('Particle filter')
xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf);
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
figure;
plot(t,abs(xArr-xhatPartArr),'b');
title('The error of PF')