卡尔曼滤波matlab实现

卡尔曼滤波

前言

一直准备搞一下这个，但是一直没定下来，这次趁此机会试一下。

原理

https://blog.csdn.net/heyijia0327/article/details/17487467这个原理推导已经很详细了
大概总结一下

经过上面一步，只有PQRK四个矩阵还未确定了。显然增益矩阵K是不需要初始化的，P是误差矩阵，初始化可以是一个随机的矩阵或者0，只要经过几次的处理基本上就能调整到正常的水平，因此也就只会影响前面几次的滤波结果。
Q和R分别是预测和观测状态协方差矩阵，一般可以简单认为系统状态各维之间(即上面的a和b)相互独立，那么Q和R就可以设置为对角阵。而这两个对角线元素的大小将直接影响着滤波结果，若Q的元素远大于R的元素，则预测噪声大，从而更相信观测值，这样可能使得kalman滤波结果与观测值基本一致；反之，则更相信预测，kalman滤波结果会表现得比较规整和平滑；若二者接近，则滤波结果介于前面两者之间，根据实验效果看也缺乏实际使用价值。
以上几个矩阵确定后，对于状态x，由于0时刻我们没有任何关于该系统的知识，可以使用0时刻的测量值z0来初始x0，预测从k=1开始；也可以初始化-1时刻的状态，当然这个状态实际是未知的，也就可随机取。2种方式都可以，但使用0时刻测量值来初始化状态，可以使得前面几次预测更准确。
https://www.cnblogs.com/jcchen1987/p/4371439.html

matlab使用

现场下了工具箱看个demo
https://blog.csdn.net/congduan/article/details/8163407

% 二维空间点
% State = (x y xdot ydot).状态 We only observe (x y).观察
% X(t+1) = F X(t) + noise(Q)
% Y(t) = H X(t) + noise(R)

ss = 4; % state size
os = 2; % observation size
F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1]; 
H = [1 0 0 0; 0 1 0 0];
Q = 0.1*eye(ss);
R = 1*eye(os);
initx = [10 10 1 0]';
initV = 10*eye(ss);

seed = 9;
rand('state', seed);
randn('state', seed);
T = 15;
[x,y] = sample_lds(F, H, Q, R, initx, T);%状态转移方程

[xfilt, Vfilt, VVfilt, loglik] = kalman_filter(y, F, H, Q, R, initx, initV);%卡尔曼滤波实现
figure(1)
clf
%subplot(2,1,1)
hold on
plot(x(1,:), x(2,:), 'ks-');
plot(y(1,:), y(2,:), 'g*');
plot(xfilt(1,:), xfilt(2,:), 'rx:');
for t=1:T, plotgauss2d(xfilt(1:2,t), Vfilt(1:2, 1:2, t)); end
hold off
legend('true', 'observed', 'filtered')
xlabel('x')
ylabel('y')

demo可以跑 ，原先的有smooth内容我删除了。

结论

看了下多传感器的融合，看起来是滤波完继续滤波，交替使用传感器，不知道理解对不对。这个demo自己到时可以试试看从头写一个算法实现。