1、75 卡尔曼滤波器7.5 卡尔曼滤波器由于维纳滤波器难以用于实时处理，满足不了20世纪50年代航天航空技术发展的要求，于是人们开始探索新的理论和技术途径。20世纪60年代新出现的卡尔曼滤波理论，用信号与噪声的状态空间模型取代了相关函数，用时域的微分方程来表示滤波问题，得到了递推估计算法，适用于计算机实时处理，它突破了维纳滤波的平稳过程的限制，也没有无限时间的要求，这一对维纳滤波理论的重大突破很快地被用于空间技术、自动控制和信号处理等领域。卡尔曼滤波由滤波方程和预测方程两部分组成。7.5.1 滤波基本方程设信号状态方程和量测方程分别为 (7-133) (7-134)式中，为信号的状态向量，为量测
2、向量，和分别为状态噪声和量测噪声，且为互不相关的高斯白噪声向量序列，其协方差分别为和；，和分别为状态转移矩阵、输入矩阵和观测矩阵。卡尔曼滤波基本方程为 (7-135) (7-136) (7-137) (7-138) (7-139)其中，残差(新息)定义为 (7-140)协方差矩阵为 (7-141)7.5.2 一步预测基本方程卡尔曼一步预测基本方程为 (7-142) (7-143) (7-144)式中，为一步预测增益阵。【例7-16】利用卡尔曼滤波器跟踪机动目标。例程7-14 基于卡尔曼滤波器的机动目标跟踪% Make a point move in the 2D plane% State =
3、(x y xdot ydot). We only observe (x y).% X(t+1) = (t) X(t) + noise(Q)% Y(t) = H X(t) + noise(R)ss = 4; % state sizeos = 2; % observation sizeF = 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; %target initial parametersinitV = 10*eye(ss);seed
4、 = 9;rand(state, seed);randn(state, seed);T = 15;x,y = sample_lds(F, H, Q, R, initx, T); %generate target data%kalman filter xfilt, Vfilt, VVfilt, loglik = kalman_filter(y, F, H, Q, R, initx, initV); % one step predictxsmooth, Vsmooth = kalman_smoother(y, F, H, Q, R, initx, initV);%calculate the err
5、or between the filtered data and the real data dfilt = x(1 2,:) - xfilt(1 2,:); mse_filt = sqrt(sum(sum(dfilt.2); dsmooth = x(1 2,:) - xsmooth(1 2,:);mse_smooth = sqrt(sum(sum(dsmooth.2)figure(1)clf%subplot(2,1,1)hold onplot(x(1,:), x(2,:), ks-);plot(y(1,:), y(2,:), g*);plot(xfilt(1,:), xfilt(2,:),
6、rx:);for t=1:T, plotgauss2d(xfilt(1:2,t), Vfilt(1:2, 1:2, t); endhold offlegend(true, observed, filtered, 3)xlabel(x)ylabel(y)% 3x3 inchesset(gcf,units,inches);set(gcf,PaperPosition,0 0 3 3) %print(gcf,-depsc,/home/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.eps);%print(gcf,-djpeg,-r100, /h
7、ome/eecs/murphyk/public_html/Bayes/Figures/aima_filtered.jpg);figure(2)%subplot(2,1,2)hold onplot(x(1,:), x(2,:), ks-);plot(y(1,:), y(2,:), g*);plot(xsmooth(1,:), xsmooth(2,:), rx:);for t=1:T, plotgauss2d(xsmooth(1:2,t), Vsmooth(1:2, 1:2, t); endhold offlegend(true, observed, smoothed, 3)xlabel(x)yl
8、abel(y)% 3x3 inchesset(gcf,units,inches);set(gcf,PaperPosition,0 0 3 3) %print(gcf,-djpeg,-r100, /home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.jpg);%print(gcf,-depsc,/home/eecs/murphyk/public_html/Bayes/Figures/aima_smoothed.eps);图7-27 卡尔曼滤波器跟踪机动目标仿真图7-27中“”表示机动目标的真实轨迹，“”表示目标观测轨迹，“”表示经卡
9、尔曼滤波后的目标轨迹，圆表示跟踪波门。可以看出，卡尔曼滤波器能较为准确地跟踪机动目标。函数kalman_filter和 kalman_update分别是实现卡尔曼滤波方程滤波和状态更新的代码。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
10、 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
11、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 mo
12、del m% OUTPUTS (where X is the hidden state being estimated)% x(:,t) = EX(:,t) | y(:,1:t)% V(:,:,t) = CovX(:,t) | y(:,1:t)% VV(:,:,t) = CovX(:,t), X(:,t-1) | y(:,1:t) t = 2% loglik = sumt=1T log P(y(:,t)% If an input signal is specified, we also condition on it:% e.g., x(:,t) = EX(:,t) | y(:,1:t), u
13、(:, 1:t)% If a model sequence is specified, we also condition on it:% e.g., x(:,t) = EX(:,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:nargs switch argsi case
14、model, model = argsi+1; case u, u = argsi+1; case B, B = argsi+1; case ndx, ndx = argsi+1; otherwise, error(unrecognized argument argsi) endendx = zeros(ss, T);V = zeros(ss, ss, T);VV = zeros(ss, ss, T);loglik = 0;for t=1:T m = model(t); if t=1 %prevx = init_x(:,m); %prevV = init_V(:,:,m); prevx = i
15、nit_x; prevV = init_V; initial = 1; else prevx = x(:,t-1); prevV = V(:,:,t-1); initial = 0; end if 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); else if isempty(ndx) x(:,t), V(:,:,t), LL, VV(:,:,t) = . ka
16、lman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, . initial, initial, u, u(:,t), B, B(:,:,m); else i = ndxt; % copy over all elements; only some will get updated x(:,t) = prevx; prevP = inv(prevV); prevPsmall = prevP(i,i); prevVsmall = inv(prevPsmall); x(i,t), smallV, LL, VV(
17、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); end end loglik = loglik + LL;endfunction xnew, Vnew, loglik, VVnew = kalman_update(A, C, Q, R,
18、y, x, V, varargin)% KALMAN_UPDATE Do a one step update of the Kalman filter% xnew, Vnew, loglik = kalman_update(A, C, Q, R, y, x, V, .)% INPUTS:% A - the system matrix% C - the observation matrix % Q - the system covariance % R - the observation covariance% y(:)- the observation at time t% x(:) - EX
19、 | y(:, 1:t-1) prior mean% V(:,:) - CovX | y(:, 1:t-1) prior covariance% OPTIONAL INPUTS (string/value pairs default in brackets)% initial - 1 means x and V are taken as initial conditions (so A and Q are ignored) 0% u- u(:) the control signal at time t % B- the input regression matrix% OUTPUTS (whe
20、re X is the hidden state being estimated)% xnew(:) =E X | y(:, 1:t) % Vnew(:,:) = Var X(t) | y(:, 1:t) % VVnew(:,:) = Cov X(t), X(t-1) | y(:, 1:t) % loglik = log P(y(:,t) | y(:,1:t-1) log-likelihood of innovatio% set default paramsu = ;B = ;initial = 0;args = varargin;for i=1:2:length(args) switch a
21、rgsi case u, u = argsi+1; case B, B = argsi+1; case initial, initial = argsi+1; otherwise, error(unrecognized argument argsi) endend% xpred(:) = EX_t+1 | y(:, 1:t)% Vpred(:,:) = CovX_t+1 | y(:, 1:t)if initial if isempty(u) xpred = x; else xpred = x + B*u; end Vpred = V;else if isempty(u) xpred = A*x
22、; else xpred = A*x + B*u; end Vpred = A*V*A + Q;ende = y - C*xpred; % error (innovation)n = length(e);ss = length(A);S = C*Vpred*C + R;Sinv = inv(S);ss = length(V);loglik = gaussian_prob(e, zeros(1,length(e), S, 1);K = Vpred*C*Sinv; % Kalman gain matrix% If there is no observation vector, set K = zeros(ss).xnew = xpred + K*e;Vnew = (eye(ss) - K*C)*Vpred;VVnew = (eye(ss) - K*C)*A*V;
