Kalman简介
Kalman滤波就是个滤波算法,能把乱七八糟抖动的信号进行滤波
大致实现的结果如图1所示,红色的是Kalman滤波结果,蓝色的是原始信号
图1
Kalman滤波流程(无脑)
如果是仅仅想用一下Kalman滤波,不想知道原理,请仅看这一节
核心公式是下面这5个
- P1(n)=FP(n−1)FT+QwP1(n)=FP(n−1)FT+Qw
- m(n)=P1(n)h[hTP1(n)h+σ2v]−1m(n)=P1(n)h[hTP1(n)h+σv2]−1
- P(n)=[I−m(n)hT]P1(n)P(n)=[I−m(n)hT]P1(n)
- ^x(1)(n)=F^x(n−1)+u(n)x^(1)(n)=Fx^(n−1)+u(n)
- ^x(n)=[I−m(n)hT]^x(1)+m(n)y(n)x^(n)=[I−m(n)hT]x^(1)+m(n)y(n)
那么这5个公式都是什么东西呢,其实不用管,只需要了解,
- (n)(n)指的是n时刻,也就是图1的坐标横轴,
- y(n)y(n)是已知的测量量,可以理解为图1的蓝线纵坐标,
- x(n)x(n)是我们滤波的结果,可以理解为图1 红线纵坐标,
- σ2vσv2是已知变量,指的是噪声,
- QwQw可以根据σ2vσv2计算的常量(协方差矩阵)
- FF是已知变量,需要根据具体问题设定,指的是x(n)x(n)与x(n−t)x(n−t)之间相互转化的矩阵【需要一点大一线性代数知识】
- hh是已知变量,需要根据具体问题设定,指的是yy与xx之间的联系,理解为y=hTx(n)+噪声y=hTx(n)+噪声
当开始滤波的时候,需要输入任意的x(0)x(0)以及P(0)P(0),所以这两个变量也是已知的
到此,再看看这些公式,为了求解x(n)x(n),哪个变量是不知道的呢?
Kalman滤波在运行的过程中,按照nn从0到期望的时刻,公式1到公式5的计算顺序,整个系统就能迭代的计算了
Kalman滤波例子(无脑)
网上的代码一大堆,找了一个比较好理解的,就是那个室内温度问题,整理了一下贴在下面
问题叙述为
- 需要测量房间的问题,有两种方法:(1)温度计;(2)经验,即根据上一刻的温度估计下一刻的温度。哪一个更准很难说,因此需要根据仪器此刻的测量值,加上以往的经验,获得当前的室内温度。
- 在这个例子中,温度计就是测量值,即yy,房间在nn时刻的温度就是x(n)x(n),我们需要运用Kalman滤波实现基于温度计的房间温度精确估计,也就是滤波温度计的测量值
- 由于温度计测量的就是房间的温度,因此h=1h=1
- 由于房间上一时刻温度和下一时刻温度是一个东西,因此F=1F=1
- 于是设定x(0)x(0)与P(0)P(0)的初始值,就可以迭代运行上面的公式了
代码为
% Kalman Filter是根据上一状态的估计值和当前状态的观测值推断当前状态估计值的方法
clear all;
N=200;
%w(1)=0;
%w=randn(1,N);
w=0; %系统控制矩阵
x(1)=0;
a=1; %温度模拟A为1
V=randn(1,N);
q1=std(V);
%Rvv=q1.^2;
Rvv=0.1; %测量过程协方差 温度模拟R为1e-1
q2=std(x);
Rxx=q2.^2;
q3=std(w);
%Rww=q3.^2;
Rww = 0.000001; %温度模拟Q为1e-6
%c=0.6;
c=1; %温度模拟H为1
for k=1:N;
Y(k)=25+sqrt(0.1)*randn(1); %温度模拟平均温度为25度 方差(协方差)为0.1的温度输入 测量方程,其中V为测量系统的噪声,c为测量系统的参数
end
p(1)=10; %协方差 初始值
s(1)=1; %最优估计 初始值
for t=2:N;
s(t)=a*s(t-1)+w; %先验估计 求当前时刻的估计值
p1(t)=a.^2*p(t-1)+Rww; %协方差估计 求当前时刻的估计值的偏差,a为系统参数,没有控制量,所以没有参数b,Rww为噪声
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); %求Kg,b(t)为Kg,即Kalman增量
s(t)=a*s(t)+b(t)*(Y(t)-a*c*s(t));%后验估计 求t时刻的最优值,即当前时刻的最优值
p(t)=p1(t)-c*b(t)*p1(t); %后验协方差 求当前最状态最优值的偏差,即式子5:(1-c*b(t))*p1(t)
end
figure(1);
plot(Y,'g--');hold on;
plot(s,'r--');hold on;