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

　　

4.3 结果显示

不带控制参数的位置点数据图谱：

不带控制参数的方差值图谱：

带控制参数的位置点数据图谱：

带控制参数的方差值图谱：

5 总结

　　性能确实还可以的，里面的难点在于各个矩阵运算以及Q、R参数的设定，这里我是使用随机参数法，广撒网多次蒙特卡洛仿真求均值，最后在这些参数中选择最优的那个解为我的模型参数。

6 相关链接

开源代码：

链接：https://pan.baidu.com/s/1fUM0VmPVabqKv89WUyZmng  提取码：x3zv

参考链接：

https://www.jianshu.com/p/f6ce8943560c?from=singlemessage

https://www.youtube.com/watch?v=2-lu3GNbXM8

滤波器算法（1）-卡尔曼滤波小车附带题目与MATLAB程序的更多相关文章

  1. 链表算法题二，还原题目，用debug调试搞懂每一道题

    文章简述 大家好,本篇是个人的第4篇文章. 承接第3篇文章<开启算法之路,还原题目,用debug调试搞懂每一道题>,本篇文章继续分享关于链表的算法题目. 本篇文章共有5道题目 一,反转链表 ...

  2. GMM算法的matlab程序

    GMM算法的matlab程序 在“GMM算法的matlab程序(初步)”这篇文章中已经用matlab程序对iris数据库进行简单的实现,下面的程序最终的目的是求准确度. 作者:凯鲁嘎吉 - 博客园 h ...

  3. GMM算法的matlab程序（初步）

    GMM算法的matlab程序 在https://www.cnblogs.com/kailugaji/p/9648508.html文章中已经介绍了GMM算法,现在用matlab程序实现它. 作者:凯鲁嘎 ...

  4. KFCM算法的matlab程序（用FCM初始化聚类中心）

    KFCM算法的matlab程序(用FCM初始化聚类中心) 在“聚类——KFCM”这篇文章中已经介绍了KFCM算法,现在用matlab程序对iris数据库进行实现,用FCM初始化聚类中心,并求其准确度与 ...

  5. KFCM算法的matlab程序

    KFCM算法的matlab程序 在“聚类——KFCM”这篇文章中已经介绍了KFCM算法,现在用matlab程序对iris数据库进行简单的实现,并求其准确度. 作者:凯鲁嘎吉 - 博客园 http:// ...

  6. FCM算法的matlab程序2

    FCM算法的matlab程序2 在“FCM算法的matlab程序”这篇文章中已经用matlab程序对iris数据库进行实现,并求解准确度.下面的程序是另一种方法,是最常用的方法:先初始化聚类中心,在进 ...

  7. FCM算法的matlab程序

    FCM算法的matlab程序 在“FCM算法的matlab程序(初步)”这篇文章中已经用matlab程序对iris数据库进行简单的实现,下面的程序最终的目的是求准确度. 作者:凯鲁嘎吉 - 博客园 h ...

  8. K-means算法的matlab程序

    K-means算法的matlab程序 在“K-means算法的matlab程序(初步)”这篇文章中已经用matlab程序对iris数据库进行简单的实现,下面的程序最终的目的是求准确度. 作者:凯鲁嘎吉 ...

  9. FCM算法的matlab程序（初步）

    FCM算法的matlab程序 在https://www.cnblogs.com/kailugaji/p/9648430.html文章中已经介绍了FCM算法,现在用matlab程序实现它. 作者:凯鲁嘎 ...

随机推荐

  1. Ubuntu16.04下升级Python到3.6.5

    本文链接:https://blog.csdn.net/chaiyu2002/article/details/82698376原帖存于IT老兵博客.Ubuntu16.04下升级Python到3.6.5 ...

  2. 阿里云ECS，Ubuntu Server 16.04安装图形界面远程控制

    最近阿里云有新用户免费体验6个月的活动,虽说是免费体验,但是还是要买个它们的产品才行,我就花9.9买了个最便宜的,然后就获得了一个乞丐版的ECS服务器,配置是1核内存1G.系统装的是Ubuntu Se ...

  3. 借助中间件优化代码 将请求RequestId在服务端接收到请求在处理业务逻辑之前生成

    将请求RequestId在服务端接收到请求在处理业务逻辑之前生成

  4. Tomcat connection & session timeout settings

    # connection timeout for globle web application cat /home/soft/apache-tomcat-7.0.92/conf/server.xml ...

  5. Andorid SQLite数据库开发基础教程（3）

    Andorid SQLite数据库开发基础教程(3) 数据库生成方式 数据库的生成有两种方式,一种是使用数据库管理工具生成的数据库,我们将此类数据库称为预设数据库,另一种是使用代码生成的数据库. 使用 ...

  6. Dart集合

    /* List里面常用的属性和方法: 常用属性: length 长度 reversed 翻转 isEmpty 是否为空 isNotEmpty 是否不为空 常用方法: add 增加 addAll 拼接数 ...

  7. Ｃ++11 学习笔记 std::function和bind绑定器

    C++11 学习笔记 std::function和bind绑定器 一.std::function C++中的可调用对象虽然具有比较统一操作形式(除了类成员指针之外,都是后面加括号进行调用),但定义方法 ...

  8. [译]在Pandas的Dataframe中删除行、列

    导入模块 import pandas as pd 创建dataframe data = {'name': ['Jason', 'Molly', 'Tina', 'Jake', 'Amy'], 'yea ...

  9. 图片缩放——利用layui的滑块

    @layui官网文档.@参考博客 参考博客中能实现,但是效果差强人意,在前辈的基础上进行了改造,并支持了动态多图列表 <%@ page language="java" con ...

  10. Python - Django - ORM 聚合查询和分组查询

    models.py: from django.db import models # 出版社 class Publisher(models.Model): id = models.AutoField(p ...