卡尔曼滤波的由来

斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑使用了这种滤波器。 关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。数据滤波是去除噪声还原真实数据的一种数据处理技术, Kalman滤波在测量方差已知的情况下能够从一系列存在测量噪声的数据中，估计动态系统的状态. 由于, 它便于计算机编程实现, 并能够对现场采集的数据进行实时的更新和处理, Kalman滤波是目前应用最为广泛的滤波方法, 在通信, 导航, 制导与控制等多领域得到了较好的应用.中文名卡尔曼滤波器，Kalman滤波，卡曼滤波外文名 KALMAN FILTER表达式X(k)=A X(k-1)+B U(k)+W(k)提出者斯坦利·施密特提出时间1958应用学科天文，宇航，气象适用领域范围雷达跟踪去噪声适用领域范围控制、制导、导航、通讯等现代工程1、背景斯坦利·施密特(Stanley Schmidt)首次实现了卡尔曼滤波器。卡尔曼在NASA埃姆斯研究中心访问时,发现他的方法对于解决阿波罗计划的轨道预测很有用,后来阿波罗飞船的导航电脑使用了这种滤波器。关于这种滤波器的论文由Swerling (1958), Kalman (1960)与 Kalman and Bucy (1961)发表。2、形式卡尔曼滤波已经有很多不同的实现，卡尔曼最初提出的形式一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在3、实例卡尔曼滤波的一个典型实例是从一组有限的，对物体位置的，包含噪声的观察序列中预测出物体的坐标位置及速度。在很多工程应用(雷达、计算机视觉)中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要话题。4、应用比如，在雷达中，人们感兴趣的是跟踪目标，但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息,设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计(滤波)，也可以是对于将来位置的估计(预测)，也可以是对过去位置的估计(插值或平滑)。扩展卡尔曼滤波（EXTEND KALMAN FILTER, EKF）扩展卡尔曼滤波器是由kalman filter考虑时间非线性的动态系统，常应用于目标跟踪系统。5、状态估计状态估计状态估计是卡尔曼滤波的重要组成部分。一般来说，根据观测数据对随机量进行定量推断就是估计问题，特别是对动态行为的状态估计，它能实现实时运行状态的估计和预测功能。比如对飞行器状态估计。状态估计对于了解和控制一个系统具有重要意义，所应用的方法属于统计学中的估计理论。最常用的是最小二乘估计，线性最小方差估计、最小方差估计、递推最小二乘估计等。其他如风险准则的贝叶斯估计、最大似然估计、随机逼近等方法也都有应用。状态量受噪声干扰的状态量是个随机量，不可能测得精确值，但可对它进行一系列观测，并依据一组观测值，按某种统计观点对它进行估计。使估计值尽可能准确地接近真实值，这就是最优估计。真实值与估计值之差称为估计误差。若估计值的数学期望与真实值相等，这种估计称为无偏估计。卡尔曼提出的递推最优估计理论，采用状态空间描述法，在算法采用递推形式，卡尔曼滤波能处理多维和非平稳的随机过程。理论卡尔曼滤波理论的提出，克服了威纳滤波理论的局限性使其在工程上得到了广泛的应用，尤其在控制、制导、导航、通讯等现代工程方面。6、MATLAB程序MATLABN=200;w(1)=0;w=randn(1,N)x(1)=0;a=1;for k=2:N;x(k)=a*x(k-1)+w(k-1);endV=randn(1,N);q1=std(V);Rvv=q1.^2;q2=std(x);Rxx=q2.^2;q3=std(w);Rww=q3.^2;c=0.2;Y=c*x+V;p(1)=0;s(1)=0;for t=2:N;p1(t)=a.^2*p(t-1)+Rww;b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));p(t)=p1(t)-c*b(t)*p1(t);endt=1:N;plot(t,s,'r',t,Y,'g',t,x,'b');function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)% Kalman filter.% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)%% INPUTS:% y(:,t) - the observation at time t% A - the system matrix% C - the observation matrix% Q - the system covariance% R - the observation covariance% init_x - the initial state (column) vector% init_V - the initial state covariance%% OPTIONAL INPUTS (string/value pairs [default in brackets])% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ]% In this case, all the above matrices take an additional final dimension,% i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m).% However, init_x and init_V are independent of model(1).% 'u' - u(:,t) the control signal at time t [ [] ]% 'B' - B(:,:,m) the input regression matrix for model m%% OUTPUTS (where X is the hidden state being estimated)% x(:,t) = E[X(:,t) | y(:,1:t)]% V(:,:,t) = Cov[X(:,t) | y(:,1:t)]% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2% loglik = sum{t=1}^T log P(y(:,t))%% If an input signal is specified, we also condition on it:% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]% If a model sequence is specified, we also condition on it:% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)][os T] = size(y);ss = size(A,1); % size of state space% set default paramsmodel = ones(1,T);u = [];B = [];ndx = [];args = varargin;nargs = length(args);for i=1:2:nargsswitch argscase 'model', model = args{i+1};case 'u', u = args{i+1};case 'B', B = args{i+1};case 'ndx', ndx = args{i+1};otherwise, error(['unrecognized argument ' args])endendx = zeros(ss, T);V = zeros(ss, ss, T);VV = zeros(ss, ss, T);loglik = 0;for t=1:Tm = model(t);if t==1%prevx = init_x(:,m);%prevV = init_V(:,:,m);prevx = init_x;prevV = init_V;initial = 1;elseprevx = x(:,t-1);prevV = V(:,:,t-1);initial = 0;endif isempty(u)[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial);elseif isempty(ndx)[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ...'initial', initial, 'u', u(:,t), 'B', B(:,:,m));elsei = ndx;% copy over all elements; only some will get updatedx(:,t) = prevx;prevP = inv(prevV);prevPsmall = prevP(i,i);prevVsmall = inv(prevPsmall);[x(i,t), smallV, LL, VV(i,i,t)] = ...kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ...'initial', initial, 'u', u(:,t), 'B', B(i,:,m));smallP = inv(smallV);prevP(i,i) = smallP;V(:,:,t) = inv(prevP);endendloglik = loglik + LL;end另一个例子：[1] Z=(1:100); %观测值noise=randn(1,100); %方差为1的高斯噪声Z=Z+noise;X=[0;0]; %状态P=[10;01]; %状态协方差矩阵F=[11;01]; %状态转移矩阵Q=[0.0001,0;00.0001]; %状态转移协方差矩阵H=[10]; %观测矩阵R=1; %观测噪声方差figure;hold on;fori=1:100X_ = F*X_ = F*P*F'+Q;K = P_*H'/(H*P_*H'+R);X = X_+K*(Z(i)-H*X_) = (eye(2)-K*H)*P_;plot(X(1), X(2)); %画点，横轴表示位置，纵轴表示速度end

很详细！