在开篇先给大家看看知乎上的一个视频，这个视频比较有意思
https://www.zhihu.com/question/23971601
https://www.bilibili.com/video/BV14K411L7yq/?spm_id_from=333.788.videocard.0
代码部分：
%%%%kalman filter
%X(K)一FX(K-1)+Q
%Y(K)-HX(K)+R
%%%第一个问题，生成一段随机信号，并滤波
%生成一段时间t
t=0.1:0.01:1;
L=length(t);
%生成真实信号x，以及观测y
%首先初始化
x=zeros(1,L);
y=x;
%生成信号，设x=t^2
for i=1:L
x(i)=t(i)^2;
y(i)=x(i)+normrnd(0,0.1);%正态分布，参数为期望和标准差
end
%%%%%%%%%%%信号生成完毕%%%%
%%%%%%%滤波算法%%%%%%
%plot(t,x,t,y,‘LineWidth’,2)
%%%预测方程观测方程怎么写%%%%观测方程好写Y(K)=X(K)+R R~N(0,1)
%预测方程不好写%%%%，在这里，可以猜一猜是线性增长，但是大多数问题，信号是杂乱无章的，怎么办%模型一，最粗糙的建模
%X(K)=X(K-1)+Q
%Y(K)-X(K)+R%猜Q~N(O,1);
F1=1;
H1=1;
Q1=1;
R1=1;%模型1的F1,H1都是一个标量（数），他不是一个矩阵，matlab认为标量就是一个1x1的矩阵
%初始化x(k)+
Xplus1=zeros(1,L);%plus＋的英语minus -的英语
%我们会经常用到Xplus,Xminus,Pplus,Pminus
%设置一个初值，假设kplus1(1)-N(0.01,0.01^2)
Xplus1(1)=0.01;
Pplus1=0.01^2;
%%%卡尔曼滤波算法
%X(K)minus=FX(K-1)plus
%P(K)minus=FP(K-1)plusF’+Q
%K=P(K)minusH’inv(HP(K)minusH’+R) K----卡尔曼增益
%X(K)plus=X(K)minus+K(y(k)-HX(K)minus)
%P(K)plus=(I-KH)P(K)minus
for i=2:L
%%%%预测步%%%%%%
Xminus1=F1Xplus1(i-1);
Pminus1=F1Pplus1F1’+Q1;
%%%%%更新步%%%%%
K1=(Pminus1H1’)inv(H1Pminus1H1’+R1);
Xplus1(i)=Xminus1+K1*(y(i)-H1Xminus1);
Pplus1=(1-K1H1)*Pminus1;
end
%matlab固有的缺陷之一：矩阵求逆后后面的几位小数会丢失
plot(t,x,‘r’,t,y,‘g’,t,Xplus1,‘b’,‘LineWidth’,2);
红色是真实的信号，绿色是传感器采到的信号，蓝色是滤波值的信号，滤波结果不是很好，
1 因为我们的滤波模型太粗糙了
2 Q选的太大了
我自己改了些参数，主要是改这四个参数
F1=1;
H1=1;
Q1=1;
R1=1;%模型1的F1,H1都是一个标量（数），他不是一个矩阵，matlab认为标量就是一个1x1的矩阵
做了对比试验 效果比较明显的是 改变F1和H1的值 大家可以自己试试改改参数
当改变Q1和R1的值的时候，我试了从0.0001到2000000不等，图的效果变化不大，但是当改变F1和H1的时候，
可见，当F1和H1越来越大的时候，滤波值的信号偏的离谱，当达到1000时几乎与初值融合
从
for i=2:L
%%%%预测步%%%%%%
Xminus1=F1Xplus1(i-1);
Pminus1=F1Pplus1F1’+Q1;
%%%%%更新步%%%%%
K1=(Pminus1H1’)inv(H1Pminus1H1’+R1);
Xplus1(i)=Xminus1+K1(y(i)-H1Xminus1);
Pplus1=(1-K1H1)Pminus1;
end
也可以看出，当F1和H1很大时，预测步中方程
Xminus1=F1Xplus1(i-1);
Pminus1=F1Pplus1F1’+Q1;
中后一时刻先验的状态和方差受上一时刻后验的状态和方差影响更大，此时Q1的影响微乎其微，几乎就是上一时刻的状态传递到了后一时刻，
更新步中方程
K1=(Pminus1H1’)inv(H1Pminus1H1’+R1);
Xplus1(i)=Xminus1+K1*(y(i)-H1Xminus1);
Pplus1=(1-K1H1)*Pminus1;
中后一时刻的后验也受后一时刻先验影响，所以影响一直这样传递了下去，Q1和R1的影响几乎可以忽略不计，系统就相当于没有了预测和观测，全靠自己的状态传递，突然脑子里就想到了信号与系统里学的零输入响应，全靠系统自己振荡，也不知道这之间有没有联系我也不懂，如果有大佬知道的话请赐教 我也不知道我分析的对不对
所以当F1和H1极大时，滤波值的信号几乎不变，滤波失效，所以F1和H1应该选取适当的值，我也不知道适当的值是多少，可能跟Q1和R1以及那些均值方差等有关？
当我只改变随机正态信号的方差由0.1改为0.5过后，信号变得更抖了，因为方差变大了这个很好理解
以上都是我根据自己的理解瞎操作的 hhh 大家过自己的脑子看吧 下面是视频里讲的 我相当于做个笔记
滤波值相对于前面来说呢要平滑一点，但是还是比较差，因为预测的模型建的太粗糙了，因为它实际上是一个平方的变化
（%生成信号，设x=t^2
for i=1:L
x(i)=t(i)^2;
y(i)=x(i)+normrnd(0,0.1);%正态分布，参数为期望和标准差
end
%%%%%%%%%%%信号生成完毕%%%%）
但是我们代码认为它是X(k)=X(k-1)加这个Q，所以我们的建模就比较糙，就不能反映这个真实的情况，所以在前面的一段滤波还是比较不错的，但是到后面这一段平方项就开始发力了快速增长了，我们的预测就跟不上了，因为真实值是平方变化的，而我们的滤波值是+Q是线性变化的，所以当我们过于相信预测值的时候，这个值就会有一个天然的往下掉的一个趋势，他离真实值就会越来越远，比真实值小。
当如图时，绿色是传感器采到的信号，蓝色是滤波值的信号，就完全相信滤波的信号（观测值），
放大后看到绿色和蓝色重合，
所以当我们把Q和R都改回1时，
预测值和观测值都趋于0，
当我们把真实的初值从0.01改成1时如上图，最后随着时间的推移可以把滤波值拉回到正确的位置，这就是一个相当不错的性质，他对初始值不敏感，所以对初值的要求没那么高，可以随便乱给，在我们这里初值是1，方差是0.01^2，方差可以说是非常的小，也就是说我们对我们的初值给的非常的自信，也就是我们主观上非常相信他的初值是1，结果卡尔曼滤波的结果显示他不会受你主观的因素所影响，他该是什么就是什么，这就是一个比较好的性质，
下面是模型2
代码：
%%%%kalman filter
%X(K)一FX(K-1)+Q
%Y(K)-HX(K)+R
%%%第一个问题，生成一段随机信号，并滤波
%生成一段时间t
t=0.1:0.01:1;
L=length(t);
%生成真实信号x，以及观测y
%首先初始化
x=zeros(1,L);
y=x;
%生成信号，设x=t^2
for i=1:L
x(i)=t(i)^2;
y(i)=x(i)+normrnd(0,0.1);%正态分布，参数为期望和标准差
end
%%%%%%%%%%%信号生成完毕%%%%
%%%%%%%滤波算法%%%%%%
%plot(t,x,t,y,‘LineWidth’,2)
%%%预测方程观测方程怎么写%%%%观测方程好写Y(K)=X(K)+R R~N(0,1)
%预测方程不好写%%%%，在这里，可以猜一猜是线性增长，但是大多数问题，信号是杂乱无章的，怎么办%模型一，最粗糙的建模
%X(K)=X(K-1)+Q
%Y(K)-X(K)+R%猜Q~N(O,1);
F1=1;
H1=1;
Q1=1;
R1=1;%模型1的F1,H1都是一个标量（数），他不是一个矩阵，matlab认为标量就是一个1x1的矩阵
%初始化x(k)+
Xplus1=zeros(1,L);%plus＋的英语minus -的英语
%我们会经常用到Xplus,Xminus,Pplus,Pminus
%设置一个初值，假设kplus1(1)-N(0.01,0.01^2)
Xplus1(1)=1;
Pplus1=0.01^2;
%%%卡尔曼滤波算法
%X(K)minus=FX(K-1)plus
%P(K)minus=FP(K-1)plusF’+Q
%K=P(K)minusH’inv(HP(K)minusH’+R) K----卡尔曼增益
%X(K)plus=X(K)minus+K(y(k)-HX(K)minus)
%P(K)plus=(I-KH)P(K)minus
for i=2:L
%%%%预测步%%%%%%
Xminus1=F1Xplus1(i-1);
Pminus1=F1Pplus1F1’+Q1;
%%%%%更新步%%%%%
K1=(Pminus1H1’)inv(H1Pminus1H1’+R1);
Xplus1(i)=Xminus1+K1*(y(i)-H1Xminus1);
Pplus1=(1-K1H1)*Pminus1;
end
%matlab固有的缺陷之一：矩阵求逆后后面的几位小数会丢失
%模型2
%X(K)-X(K-1)+X’(K-1)dt+X"(K-1)dt^2(1/2!)+Q2
%Y(K)-X(K)+R R~N(0,1)
%此时状态变量X=[X(K)X’(K)X"(K) ]T(列向量)
%Y(k)-HX+R H=1 0 0(因为我们这里只有对X(k)的观测没有对他的倒数的观测所以是{1 0 0}）
%观测方程的H=[1 0 0]
%预测方程
%X(K)-X(K-1)+X’(K-1)dt+X"(K-1)dt^2(1/2!)+Q2
%X’(K)=0X(K-1)+X’(K-1)+X"(K-1)dt+Q3
%X"(K)=OX(K-1)+OX(K-1)+X"(K-1)+Q3 %%多项式信号多求几阶导数，总会比较平缓，而%X""(K)=X"(K-1)+Q3正是描述平缓的随机过程，这种建模相对精细一些，适用范围也较广
%F= 1 dt 0.5dt^2
% 0 1 dt
% 0 0 1
%H=[1 0 0]
%Q=Q2 0 0
% 0 Q3 0
% 0 0 Q4 Q—协方差矩阵
dt=t(2)-t(1);
F2=[1,dt,0.5dt^2;0,1,dt;0,0,1];%此处要注意矩阵是否病态，若dt特别小，易导致矩阵病态或精度丢失:
%比如dt=0.001取三次方后就会很小，matlab精度没有那么高就有可能直接把他舍弃了导致丢失了一个维度
%最后导致运行错误，这也是matlab的一个不足之处，先天的缺陷
H2=[1,0,0];
Q2=[1,0,0;0,0.01,0;0,0,0.0001];
R2=20;
%%%设置初值%%%%
Xplus2=zeros(3,L);%为什么是3，L而不是L,3呢？因为我们要保证每一个Xplus2是一个列向量
Xplus2(1,1)=0.1^2;
Xplus2(2,1)=0;
Xplus2(3,1)=0;
Pplus2=[0.01,0,0;0,0.01,0;0,0,0.0001];
for i=2:L
Xminus2=F2Xplus2(:,i-1);%第i-1列的所有元素
Pminus2=F2Pplus2F2’+Q2;
%%%更新步%%%%
K2=(Pminus2H2’)inv(H2Pminus2H2’+R2);
Xplus2(:,i)=Xminus2+K2*(y(i)-H2Xminus2);
Pplus2=(eye(3)-K2H2)*Pminus2;%eye（3）是一个3x3的单位矩阵
end
plot(t,x,‘r’,t,y,‘g’,t,Xplus1,‘b’,t,Xplus2(1,:),‘m’,‘LineWidth’,2);
%%%可以进行在线滤波，实时滤波%%%%
%问题2，两个传感器，进行滤波
%Y1(K)-X(K)+R
%Y2(K)-X(K)+R
%H-[1 1]T (列向量)X=x(K) X是一个三维的矩阵，记住X,Y都是列向量
%H= 1 0 0 X=X(K) X’(K) X"(K)
% 1 0 0
%如果是模型1的话F就是1，如果是模型2的话F就是一个三维的矩阵
蓝色是模型1的滤波结果
粉红色是模型2的滤波结果
模型2比模型1要稍微好点 这就证明了精细的模型确实对滤波会有帮助，将精度提高了不少
当把方差从0.1改为1后效果就更看得出来了，
如果单看粉红色线条对比真实值你可能觉得滤的很烂，但是你要和你的绿色传感器观测值来对比，这已经效果很好了，与绿色杂乱无章的信号相比已经得到了很好的效果了，
除了这个贝叶斯滤波还有傅里叶变换，先变换到频域把高频部分去掉再逆变换回来能得到，而且这个效果要比你的贝叶斯变换要好很多，然而贝叶斯的优点在于可以实时地进行在线的滤波，实时地计算，它是递推的，傅里叶变换进行滤波是不可以的，因为你必须首先把信号先生成出来，生成完了之后再进行傅里叶变换再滤波把噪声去掉再逆变换回来，所以他不能进行一个在线的实时地滤波，但是贝叶斯滤波是可以的，所以在我们的***动态场景的状态估计中***，贝叶斯滤波用的是比较广泛的
接下来是问题2，两个传感器，进行滤波
首先把代码改了
%生成信号，设x=t^2
for i=1:L
x(i)=t(i)^2;
y(i)=x(i)+normrnd(0,1);%正态分布，参数为期望和标准差
y2(i)=x(i)+normrnd(0,1);
end
%%%%%%%%%%%信号生成完毕%%%%
最后up主会把源代码放到他的专栏，两个传感器就意味着有两个观测方程，
***记住X,Y都是列向量***Q和R一定是矩阵而且是方阵（协方差矩阵）,Q和R的维数取决于X和Y的维数，如果你的X是一个三维的列向量，那么我的Q就是3x3的，如果你的Y是2x1的列向量，那么我们的R就是2x2的，记住这几个应该就问题不大了，
好了我太傻比了朋友们，up主在自己的专栏里放了代码我把网址贴出来
https://www.bilibili.com/read/cv5562164
等会儿研究下两个传感器融合
粉红色是两个传感器进行滤波的结果
黑色是模型二的结果
我感觉两个效果差不多
明天晚七点又要每周汇报了
我就拿这个做ppt吧
ppt做好了上传一下
上传不了我就一张一张来吧hhh
昨天晚上祥妹过来看我在学卡尔曼滤波就来问我关于卡尔曼滤波的东西
我写写自己粗浅的理解
1 先给大家介绍一下百度百科里的关于卡尔曼滤波的通俗易懂的解释
我粘贴上来
简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。
卡尔曼滤波器的介绍 ：
为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。但是，他的5条公式是其核心内容。结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。
在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。
假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分布（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的实际温度值。首先你要根据k-1时刻的温度值，来预测k时刻的温度。因为你相信温度是恒定的，所以你会得到k时刻的温度预测值是跟k-1时刻一样的，假设是23度，同时该值的高斯噪声的偏差是5度（5是这样得到的：如果k-1时刻估算出的最优温度值的偏差是3，你对自己预测的不确定度是4度，他们平方相加再开方，就是5）。然后，你从温度计那里得到了k时刻的温度值，假设是25度，同时该值的偏差是4度。
由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的协方差(covariance)来判断。因为Kg=52/(52+4^2)，所以Kg=0.61，我们可以估算出k时刻的实际温度值是：23+0.61*(25-23)=24.22度。可以看出，因为温度计的协方差(covariance)比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。
现在我们已经得到k时刻的最优温度值了，下一步就是要进入k+1时刻，进行新的最优估算。到现在为止，好像还没看到什么自回归的东西出现。对了，在进入k+1时刻之前，我们还要算出k时刻那个最优值（24.22度）的偏差。算法如下：((1-Kg)*52)0.5=3.12。这里的5就是上面的k时刻你预测的那个23度温度值的偏差，得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。
就是这样，卡尔曼滤波器就不断的把协方差(covariance)递归，从而估算出最优的温度值。他运行的很快，而且它只保留了上一时刻的协方差(covariance)。上面的Kg，就是卡尔曼增益（Kalman Gain）。他可以随不同的时刻而改变他自己的值，是不是很神奇！
有点晕
1 权重会不会发生变化？
读完上面的关于温度的例子就知道会发生变化，进入下一时刻后得出的3.12就是进入k+1时刻以后k时刻估算出的最优温度值的偏差（对应于上面的3）。所以后面的一系列计算都会改变，先是高斯噪声的偏差改变,3变成了3.12，然后Kg变，估算出的温度也变再下一时刻的最优值的偏差也变，如此循环迭代
2 初始值怎么选择？
预测值是根据经验选择的，就像我们估计今天是23度一样，是主观人为选定的，观测值则是温度计得到的，选择精度越高的温度计获得的初始观测值越准
3 迭代步长有没有影响？
步长（间隔）是初始就设置好了的，就像代码那样
%生成一段时间t
t=0.1:0.01:1;
L=length(t);
步长就是0.01，从0.1开始到1一共有90步
好了我要开始学习第九讲粒子滤波了
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
