滤波器算法（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);
endfor 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);
endfor 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];
endX_=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];
endX_=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