[记录]初学kalman滤波算法

来源：互联网发布：吉他制谱软件编辑：程序博客网时间：2024/06/04 19:14

按照《kalman滤波算法从原理到实现》，自己尝试写了一个小matlab程序。！

function v =kalman_filter(s)if ~isfield(s,'x'); s.x=nan*z; endif ~isfield(s,'p'); s.p=nan; endif ~isfield(s,'z'); error('Observation vector missing'); endif ~isfield(s,'u'); s.u=0; endif ~isfield(s,'A'); s.A=eye(length(x)); endif ~isfield(s,'B'); s.B=0; endif ~isfield(s,'Q'); s.Q=zeros(length(x)); endif ~isfield(s,'R'); error('Observation covariance missing'); endif ~isfield(s,'H'); s.H=eye(length(x)); end%保证s被初始化s.I = eye(length(s.x));%第一步：预测s.x = s.A * s.x + s.B * s.u;s.p = s.A*s.p*(s.A') + s.Q;%第二步：修正K = s.p*(s.H')*(inv(s.R + s.H*s.p*s.H'));s.x = s.A*s.x + K*(s.z - s.H*s.x);s.p = s.p - K*s.H*s.p;v= s.x;end

%状态矢量a = F/m;%观测矢量s.x = 10;s.p = 2;s.R = 2^2;s.Q = 2^2;s.H = 1;s.A = 1;s.B =0;s.u = 0;v = [0,0];me = [];for n = 1:100s.z = randn*2+10;v(1,n) = kalman_filter(s);me(1,n) = s.z ;hold onplot(n,(s.z),'*')endhold onplot(v(2:end-1),'r')% hold on% plot(me(1:end-1),'g')

-----分割线，2017-5-19 记录一下

阅读全文

0 0