从零开始码出卡尔曼滤波代码-BY忠厚老实的老王

tips

用的融合方法被称为“并行滤波融合”（又称观测值扩维融合），是集中式融合系统中的一种方法。集中式融合系统还包含两种方法：序贯滤波融合（又称顺序滤波融合）和数据压缩滤波融合（又称观测值加权融合）。与集中式融合系统对应的还有分布式融合系统。

更多关于多传感器数据融合的内容，大家可参考韩崇昭老师的《多源信息融合》第二版。

---

目标：生成一段随机信号，并滤波

这里只是对数据进行滤波，并没有进行姿态什么的计算。

1 生成数据，以及对应的观测的数据

t=0.1:0.01:1;
L=length(t);
%生成真实信号x，以及观测y
%首先初始化
x=zeros(1,L);  
y=x;

y2=x;

%生成信号，设x=t^2

for i=1:L
    x(i)=t(i)^2;
    y(i)=x(i)+normrnd(0,0.1);%正态分布，参数为期望和标准差
    y2(i)=x(i)+normrnd(0,0.1);
end

%%%%%%%%%%%信号生成完毕%%%%  

plot(t,x,t,y,'LineWidth',2);

真实数据和观测数据的图示：

实际的时候，我们不知道蓝色代表的真实值，只有红色的观测值

2 滤波实现一 粗糙的实现

2.1 观测方程：当前的状态+噪声

%观测方程好写Y(K)=X(K)+R，R~N(0,1)

2.2 预测方程实现：最粗糙的建模 当前的状态=上一步的状态+噪声

%%%预测方程观测方程怎么写%%%
%预测方程不好写，因为根据上面图示的观测数据，感觉像是线性增长的，所以我们猜是线性增长
%在这里，可以猜一猜是线性增长，但是大多数问题，信号是杂乱无章的，怎么办？方法是尝试各种模型

%模型一，最粗糙的建模

%状态和观测方程
%X(K)=f * X(K-1)+Q
%Y(K)=h *X(K)+R
%猜R,Q~N(0,1); 作 

%卡尔曼滤波的前提是：状态和观测都是线性变化的,这里的f,h设为1

F1=1;
H1=1;

%猜R，Q~N(0,1),期望为0，方差是1的正态分布，Q，R是贝叶斯滤波的难点，没什么规律，只能靠自己观察总结
Q1=1;
R1=1;

2.3 初始步：给一个初值（只需要做一次）

%初始化x(k)+
Xplus1=zeros(1,L);%plus + 的英语 minus -的英语
%我们会经常用到Xplus,Xminus,Pplus,Pminus 
%设置一个初值，假设Xplus1(1)~N(0.01,0.01^2)

Xplus1(1)=0.01;%一开始的状态
Pplus1=0.01^2; %预测方差的初值

2.2.2 更新步和预测步：其实就是算矩阵形式的卡尔曼的5条公式：

%X(K)minus=F*X(