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. elementUI 列表里面含有多选框，当翻页的时候依然保持之前页多选不变

    el-table的type="selection"的使用 场景:el-table,type="selection"时,重新请求后,设置列表更新前的已勾选项 踩坑 ...

  2. 009 轮播图，offset系列

    关于使用JS做轮播图,使用一个章节进行笔迹. 一:简单轮播图 1.程序 <!DOCTYPE html> <html lang="en"> <head& ...

  3. linux下如何制作initramfs镜像?

    1. 准备文件 加入已经准备好了所有文件在/home/initrd-base目录下 2. 在内核中指定/home/initramfs-base目录 General setup -> (/home ...

  4. Leetcode: Rotated Digits

    X is a good number if after rotating each digit individually by 180 degrees, we get a valid number t ...

  5. Vue 及双向数据绑定 Vue事件 以及Vue中的ref获取dom节点

    <template> <div id="app"> <h2>{{msg}}</h2> <input type="te ...

  6. 一、postman简介

    一.场景 1.开发接口的时候需要快速的调用接口,以便调试 2.测试的时候需要非常方便的调用接口,通过不同的参数去测试接口的输出 3.这些接口调用是需要保存下来的反复运行的 4.在运行过程中如果有断言( ...

  7. Flutter中的Container和Text组件的常用属性

    效果图: import 'package:flutter/material.dart'; void main() { runApp(MyApp()); } class MyApp extends St ...

  8. Spring Cloud Hystrix Dashboard的使用 5.1.3

      Hystrix除了可以对不可用的服务进行断路隔离外,还能够对服务进行实时监控.Hystrix可以实时.累加地记录所有关于HystrixCommand的执行信息,包括每秒执行多少.请求成功多少.失败 ...

  9. jenkins中点击增加云没反应

    问题:非容器版jenkins中无法增加云 分析: 之前在jenkins中找自带的Kubernetes 插件找不到,所以就下载Kubernetes 插件进行离线安装,明明显示安装成功了,仍然不能增加云, ...

  10. [转]Microsoft VS Code 改变默认文字编码

    概要:文件->首选项->设置 输入: "files.autoGuessEncoding": true, 然后勾上. 链接地址:https://jingyan.baidu ...