滤波器算法（1）-卡尔曼滤波小车附带题目与MATLAB程序

1 简介

　　由卡尔曼这个学者提出的最佳线性滤波器，单纯时域维度即可实现【无需进行频域变换】

2 思路

　　由上一时刻的最佳估计值XKE_P预测①当前时刻预测值Pxv　　与　　②当前时刻的测量值Mxv　　进行联立计算获得当　　③前时刻的最佳估计值XKE

3 核心

4 Matlab实例

4.1 题目【老师留的课堂作业】

研一的时候做过一次，当时没有总结；最近师弟们在写这个作业花时间重新弄了一遍，做了一次总结

4.2 源代码

不带BU参数

version9_release.m

%% 卡尔曼滤波的发布版本程序
%% 时间：2019.12.05
%% 版本：v9
%% 特性：单参处理【防止多维度计算混乱】
%% TODO：引入多参数进一步优化算法
 
%% 数据读取
% MDistance=importdata('RoverMeasurementData.txt');
Mxv=importdata('data3.mat'); % 笛卡尔坐标系，测量值
Txv=importdata('RoverTrueStates.txt'); % 真实值
len=size(Mxv,1);
 
%% 参数设置
q=0.010000 ; R=0.500000;
 
Q=[1 0;0 1].*q;
 
 
%% 正式处理
XKE=zeros(4,len);
for i=1:len
    xv = func_kalmanFilter_singleVal(Mxv(i,1),Q,R,i);
    XKE(1,i)=xv(1);
    XKE(3,i)=xv(2);
end
 
for i=1:len
    xv = func_kalmanFilter_singleVal(Mxv(i,2),Q,R,i);
    XKE(2,i)=xv(1);
    XKE(4,i)=xv(2);
end
 
%% 图谱显示
figure(1);
plot(Txv(1,:),Txv(2,:),'r-*','markersize',8),hold on;
plot(XKE(1,:),XKE(2,:),'b-s','markersize',8),hold on;
plot(Mxv(:,1),Mxv(:,2),'g-v','markersize',8),hold on;
xlabel('位置x');
ylabel('位置y');
legend({'真实值','估计值','测量值'},'Location','northwest');%
set(gca,'FontSize',25);
 
figure(2);
sub_XKE=sqrt((XKE(1,:)-Txv(1,:)).^2+(XKE(2,:)-Txv(2,:)).^2);
sub_Mxv=sqrt((Mxv(:,1)'-Txv(1,:)).^2+(Mxv(:,2)'-Txv(2,:)).^2);
plot(sub_XKE,'b-s','markersize',8),hold on;
plot(sub_Mxv,'g-v','markersize',8),hold on;
xlabel('时间t');
ylabel('与真实值的距离均方差d');
legend({'估计值误差','测量值误差'},'Location','northwest');%
set(gca,'FontSize',25);
% print -djpeg -r600 不带BU位置点;
% print -djpeg -r600 不带BU方差值;

version10_release.m

%% 卡尔曼滤波的发布版本程序
%% 时间：2019.12.05
%% 版本：v9
%% 特性：单参处理【防止多维度计算混乱】
%% TODO：引入多参数进一步优化算法
 
%% 数据读取
% MDistance=importdata('RoverMeasurementData.txt');
Mxv=importdata('data3.mat'); % 笛卡尔坐标系，测量值
Txv=importdata('RoverTrueStates.txt'); % 真实值
 
%% 参数设置
MCN=20;
d_range1=0.01;
d_range2=0.01;
 
q=0:d_range1:1;
R=0:d_range2:0.5;
 
len1=size(q,2);
len2=size(R,2);
 
Q=[1 0;0 1];
 
%% 正式处理
sub_map=zeros(1,len1*len2);
len=len1*len2;
tic;
t1=toc;
% for k=1:len
parfor k=1:len
    i=mod(k-1,len1)+1;         %行号
    j=floor((k-1)/len1)+1;     %列号
    sub_map(k)=func_kalmanFilter_doubleVal(Mxv,Txv, Q.*q(1,i), R(1,j), MCN);
    fprintf('run is going on k=%d,index=(%d, %d)\n',k,i,j);
end
t2=toc;
fprintf('耗时t=%f\n',(t2-t1));
 
%% 获取当前最优解
sub_min=min(sub_map);
k=find(sub_map==sub_min);
i=mod(k-1,len1)+1;         %行号
j=floor((k-1)/len1)+1;     %列号
Eq=q(1,i);
ER=R(1,j);
fprintf('当前最优解 q=%f ; R=%f ,sub=%f\n',Eq,ER,sub_min);
 
%% 显示图谱
sub_map=reshape(sub_map,len1,len2);
[X,Y]= meshgrid(q,R);
figure(1);
 
mesh(X',Y',sub_map);
xlabel('Q预测模型噪声');
ylabel('R观测噪声');
 
%% 最优数值解1
% q=0.017;
% R=3.6;
% sub=32.0530;
 
% 当前最优解 q=0.010000 ; R=0.500000 ,sub=44.573339

带BU参数

version11_release.m

%% 卡尔曼滤波的发布版本程序
%% 时间：2019.12.05
%% 版本：v11
%% 特性：引入加速度参与计算
%% TODO：引入多参数进一步优化算法
 
%% 数据读取
% MDistance=importdata('RoverMeasurementData.txt');
Mxv=importdata('data3.mat'); % 笛卡尔坐标系，测量值
Txv=importdata('RoverTrueStates.txt'); % 真实值
len=size(Mxv,1);
 
%% 参数设置
% q=0.006000;
% R=1.270000 ;
% q=0.006000 ; R=0.010000 ;
% q=0.030000 ; R=0.050000 ;
% q=0.030000 ; R=0.050000;
q=0.040000 ; R=0.050000;
Q=[1 0;0 1].*q;
 
 
%% 正式处理
XKE=zeros(4,len);
for i=1:len
    xv = func_kalmanFilter_singleVal_withBU(Mxv(i,1),Q,R,i);
    XKE(1,i)=xv(1);
    XKE(3,i)=xv(2);
end
 
for i=1:len
    xv = func_kalmanFilter_singleVal_withBU(Mxv(i,2),Q,R,i);
    XKE(2,i)=xv(1);
    XKE(4,i)=xv(2);
end
 
%% 图谱显示
figure(1);
plot(Txv(1,:),Txv(2,:),'r-*','markersize',8),hold on;
plot(XKE(1,:),XKE(2,:),'b-s','markersize',8),hold on;
plot(Mxv(:,1),Mxv(:,2),'g-v','markersize',8),hold on;
xlabel('位置x');
ylabel('位置y');
legend({'真实值','估计值','测量值'},'Location','northwest');%
set(gca,'FontSize',25);
 
figure(2);
sub_XKE=sqrt((XKE(1,:)-Txv(1,:)).^2+(XKE(2,:)-Txv(2,:)).^2);
sub_Mxv=sqrt((Mxv(:,1)'-Txv(1,:)).^2+(Mxv(:,2)'-Txv(2,:)).^2);
plot(sub_XKE,'b-s','markersize',8),hold on;
plot(sub_Mxv,'g-v','markersize',8),hold on;
xlabel('时间t');
ylabel('与真实值的距离均方差d');
legend({'估计值误差','测量值误差'},'Location','northwest');%
set(gca,'FontSize',25);
% print -djpeg -r600 带BU位置点;
% print -djpeg -r600 带BU方差值;

version12_release.m

%% 卡尔曼滤波的发布版本程序
%% 时间：2019.12.05
%% 版本：v9
%% 特性：单参处理【防止多维度计算混乱】
%% TODO：引入多参数进一步优化算法
 
%% 数据读取
% MDistance=importdata('RoverMeasurementData.txt');
Mxv=importdata('data3.mat'); % 笛卡尔坐标系，测量值
Txv=importdata('RoverTrueStates.txt'); % 真实值
 
%% 参数设置
MCN=20;
d_range1=0.01;
d_range2=0.01;
 
q=0:d_range1:1;
R=0:d_range2:0.5;
 
len1=size(q,2);
len2=size(R,2);
 
Q=[1 0;0 1];
 
%% 正式处理
sub_map=zeros(1,len1*len2);
len=len1*len2;
tic;
t1=toc;
% for k=1:len
parfor k=1:len
    i=mod(k-1,len1)+1;         %行号
    j=floor((k-1)/len1)+1;     %列号
    sub_map(k)=func_kalmanFilter_doubleVal_withBU(Mxv,Txv, Q.*q(1,i), R(1,j), MCN);
    fprintf('run is going on k=%d,index=(%d, %d)\n',k,i,j);
end
t2=toc;
fprintf('耗时t=%f\n',(t2-t1));
 
%% 获取当前最优解
sub_min=min(sub_map);
k=find(sub_map==sub_min);
i=mod(k-1,len1)+1;         %行号
j=floor((k-1)/len1)+1;     %列号
Eq=q(1,i);
ER=R(1,j);
fprintf('当前最优解 q=%f ; R=%f ,sub=%f\n',Eq,ER,sub_min);
 
%% 显示图谱
sub_map=reshape(sub_map,len1,len2);
[X,Y]= meshgrid(q,R);
figure(1);
 
mesh(X',Y',sub_map);
xlabel('Q预测模型噪声');
ylabel('R观测噪声');
 
%% 最优数值解1
% q=0.017;
% R=3.6;
% sub=32.0530;
 
% 当前最优解 q=0.040000 ; R=0.050000 ,sub=57.511923

func_kalmanFilter_doubleVal

%% 参数比较数据Q、R的数据比较
 
 
function [sub_mean] = func_kalmanFilter_doubleVal(Mxv,Txv, Q, R, MCN)
  
sub_cell=zeros(1,MCN);
len=size(Mxv,1);
XKE=zeros(4,len);
for iRun=1:MCN
     
    %% 处理数据
    for i=1:len
        xv = func_kalmanFilter_singleVal(Mxv(i,1),Q,R,i);
        XKE(1,i)=xv(1);
        XKE(3,i)=xv(2);
    end
 
    for i=1:len
        xv = func_kalmanFilter_singleVal(Mxv(i,2),Q,R,i);
        XKE(2,i)=xv(1);
        XKE(4,i)=xv(2);
    end
     
    %% 计算均方差和
    sub=sqrt((XKE(1,end-50:end)-Txv(1,end-50:end)).^2+(XKE(2,end-50:end)-Txv(2,end-50:end)).^2);
    sub_cell(1,iRun)=sum(sub);
end
sub_mean=mean(sub_cell);
 
end

func_kalmanFilter_singleVal

%% 单参卡尔曼滤波函数
 
function [XKE] = func_kalmanFilter_singleVal(Z,Q,R,iLoop)
%FUNC_KALMANFILTER_ 此处显示有关此函数的摘要
%   此处显示详细说明
persistent X;
persistent P;
persistent F;
persistent H;
if iLoop==1
    X=[0;0];
    P=[1 0;0 1];
    F=[1 1;0 1];
    H=[1 0];
end
 
X_=F*X;
P_=F*P*F'+Q;
K=P_*H'/(H*P_*H'+R);
X=X_+K*(Z-H*X_);
P=(eye(2)-K*H)*P_;
XKE=X;
end

func_kalmanFilter_doubleVal_withBU

%% 参数比较数据Q、R的数据比较
 
 
function [sub_mean] = func_kalmanFilter_doubleVal_withBU(Mxv,Txv, Q, R, MCN)
  
sub_cell=zeros(1,MCN);
len=size(Mxv,1);
XKE=zeros(4,len);
for iRun=1:MCN
     
    %% 处理数据
    for i=1:len
        xv = func_kalmanFilter_singleVal_withBU(Mxv(i,1),Q,R,i);
        XKE(1,i)=xv(1);
        XKE(3,i)=xv(2);
    end
 
    for i=1:len
        xv = func_kalmanFilter_singleVal_withBU(Mxv(i,2),Q,R,i);
        XKE(2,i)=xv(1);
        XKE(4,i)=xv(2);
    end
     
    %% 计算均方差和
    sub=sqrt((XKE(1,end-50:end)-Txv(1,end-50:end)).^2+(XKE(2,end-50:end)-Txv(2,end-50:end)).^2);
    sub_cell(1,iRun)=sum(sub);
end
sub_mean=mean(sub_cell);
 
end

func_kalmanFilter_singleVal_withBU

%% 带加速度参数的卡尔曼滤波器
 
function [XKE] = func_kalmanFilter_singleVal_withBU(Z,Q,R,iLoop)
%FUNC_KALMANFILTER_SINGLEVAL_WITHBU 此处显示有关此函数的摘要
%   此处显示详细说明
persistent X;   % 位置与速度
persistent U;   % 加速度
persistent P;   % 协方差矩阵
persistent F;   % 状态转移矩阵
persistent B;   % 状态控制矩阵
persistent H;   % 观测矩阵
if iLoop==1
    X=[0;0];
    U=0;
    P=[1 0;0 1];
    F=[1 1;0 1];
    B=[1./2;1];
    H=[1 0];
end
 
X_=F*X+B*U;             % ① 状态预测公式
P_=F*P*F'+Q;            % ② 噪声协方差传递
K=P_*H'/(H*P_*H'+R);    % ③ 卡尔曼系数计算
XK=X;%暂存前一时刻数据
X=X_+K*(Z-H*X_);        % ④ 计算最优估计值
P=(eye(2)-K*H)*P_;      % ⑤ 噪声协方差矩阵更新
U=X(2,1)-XK(2,1);
XKE=X;
 
end