卡尔曼滤波及卡尔曼平滑不等间距示例

在之前的一篇博客里（http://blog.csdn.net/foreseerwang/article/details/77883616）介绍了卡尔曼滤波和卡尔曼平滑的理解，后来发现，MLaPP这本书的第18章里已有介绍

本文给出了一个更加完整的卡尔曼滤波和卡尔曼平滑子程序，并基于该子程序，给出观测点序列间不等间距情况下的卡尔曼滤波和卡尔曼平滑应用示例。

如下是结果：

卡尔曼滤波和卡尔曼平滑子程序（matlab）：

function [mu, si] = funKalman(y, para, KalmanMode, varargin)
%
% Kalman filter and smoother
%
% refer to MLaPP chapter 18.3
% 2018.1
% blog: http://blog.csdn.net/foreseerwang
% QQ: 50834
%
% [mu, si] = funKalman(y, parameters, KalmanMode, ...)
%
% 针对如下LDS问题，已知y序列，求解z序列
% z(1) ---- z(2) --...-- z(t) --...-- z(T)
%  |         |            |             |
%  |         |            |             |
% y(1)      y(2)         y(t)         y(T)
% 
% z(t+1) = A*z(t) + N(0, Q)
% y(t) = C*z(t) + N(0, R)
% 暂不考虑u(t)，如果需要，很容易加进去
%
% 输入:
% y - M * T，观测值序列
% para.A.value - N * N 或 N * N * (T-1)，系统转移矩阵
% para.A.model - 1 * N，可选，标注para.A的size，default ones(1,T-1)为N*N，1:T为N*N*(T-1)；
% para.C.value - M * N 或 M * N * (T-1)，观测矩阵
% para.C.model - 1 * N，可选，标注para.C的size，default ones(1,T-1)为M*N，1:T为M*N*(T-1)；
% para.Q.value - N * N 或 N * N * (T-1)，系统噪声协方差矩阵
% para.Q.model - 1 * N，可选，标注para.Q的size，default ones(1,T-1)为N*N，1:T为N*N*(T-1)；
% para.R.value - M * M 或 M * M * (T-1)，观测噪声协方差矩阵
% para.R.model - 1 * N，可选，标注para.R的size，default ones(1,T-1)为M*M，1:T为M*M*(T-1)；
% KalmanMode 
%   - 0，只进行滤波
%   _ 1，滤波+平滑
%
% 可选输入：
% init_mu - 初始均值，N * 1
% init_si - 初始方差，N * N
%
% 输出：
% mu - N * T，z序列的均值
% si - N * N * T，z序列的协方差矩阵
% if KalmanMode == 0,
%   mu(:,t) = E[z(:,t) | y(:,1:t)]
%   si(:,:,t) = Cov[z(:,t) | y(:,1:t)]
% elseif KalmanMode == 1,
%   mu(:,t) = E[z(:,t) | y(:,1:T)]
%   si(:,:,t) = Cov[z(:,t) | y(:,1:T)]
% end

[~, T] = size(y);

A = para.A.value;
if length(fieldnames(para.A)) == 1,
    Aidx = ones(1,T-1);
else
    Aidx = para.A.model;
end;

C = para.C.value;
if length(fieldnames(para.C)) == 1,
    Cidx = ones(1,T-1);
else
    Cidx = para.C.model;
end;

Q = para.Q.value;
if length(fieldnames(para.Q)) == 1,
    Qidx = ones(1,T-1);
else
    Qidx = para.Q.model;
end;

R = para.R.value;
if length(fieldnames(para.R)) == 1,
    Ridx = ones(1,T-1);
else
    Ridx = para.R.model;
end;

N = size(A,1);
mu = zeros(N, T);
si = zeros(N, N, T);
mu_bar = zeros(N, T-1);
si_bar = zeros(N, N, T-1);

if nargin>3,
    mu(:,1) = varargin{1};
    si(:,:,1) = varargin{2};
else
    mu(:,1) = pinv(C)*y(:,1);
    si(:,:,1) = diag(randn(1,N));
end;

for t = 2:T,
    mu_bar(:,t-1) = A(:,:,Aidx(t-1))*mu(:,t-1);
    si_bar(:,:,t-1) = A(:,:,Aidx(t-1))*si(:,:,t-1)*A(:,:,Aidx(t-1))' + ...
        Q(:,:,Qidx(t-1));

    %计算卡尔曼增益
    K = si_bar(:,:,t-1)*C(:,:,Cidx(t-1))'*...
        pinv(C(:,:,Cidx(t-1))*si_bar(:,:,t-1)*C(:,:,Cidx(t-1))' + ...
        R(:,:,Ridx(t-1)));

    % 根据卡尔曼增益和当前观测值修订预测值，获得当前最优预测值
    mu(:,t) = mu_bar(:,t-1) + K*(y(:,t)-C(:,:,Cidx(t-1))*mu_bar(:,t-1));
    si(:,:,t) = (diag(ones(N,1))-K*C(:,:,Cidx(t-1)))*si_bar(:,:,t-1);
end;

if KalmanMode,
    mu_ks = mu;
    si_ks = si;
    for t = T-1:-1:1,
        J = si(:,:,t)*A(:,:,Aidx(t))'/si_bar(:,:,t);            % MLaPP (18.59)
        
        mu_ks(:,t) = mu(:,t) + J*(mu_ks(:,t+1)-mu_bar(:,t));    % MLaPP (18.57)
        si_ks(:,:,t) = si(:,:,t) + J*(si_ks(:,:,t+1)-si_bar(:,:,t))*J'; % MLaPP (18.58)
    end;
    mu = mu_ks;
    si = si_ks;
end;

end

上述示例主程序

clear all; close all; rng(0);
%% v2
% 尝试不等间距采样

%% 
% blog: http://blog.csdn.net/foreseerwang
% QQ: 50834

%% 初始化参数
T = 300;            % 计算连续T个时刻
n = 2;              % 隐变量[温度，变化率]
m = 1;              % 观测量[温度]

% 形成有跳变（或者说观测间隔不等间距）的温度变化序列
tmp1 = sort(randsample(T*5, T/2)');
tmp2 = sort(randsample(T*15:T*20, T/2));
tmp_idx = [tmp1,tmp2];

% 生成数据
x = zeros(n, T);                % 隐变量，每个时刻包含两个值：温度，变化率
xrate=0.005;                    % 温度变化率
xreal = 24+tmp_idx*xrate;       % 温度变化间隔随机化***************

z = xreal + randn(m,T);    % 观测到的温度，误差方差为1

kf_para.A.value=zeros(n,n,T);      % 状态转移矩阵
% | 1，两次观测点之间的时间间隔 |
% | 0，1                      |
for nn = 1:T-1,
    kf_para.A.value(:,:,nn) = [1,tmp_idx(nn+1)-tmp_idx(nn);0,1];
end;
kf_para.A.model = 1:T-1;   % 用于表明A矩阵随时间变化，维度为：n*n*T

kf_para.C.value = zeros(m,n);         % 观测矩阵，把m维观测量转换成n维状态量
kf_para.C.value(1)=1;
kf_para.Q.value = [1e-4, 0; 0, 1e-8]; % 预测误差的方差
kf_para.R.value = 0.25;               % 观察误差的方差

%% 卡尔曼滤波和卡尔曼平滑
[x_kf, ~] = funKalman(z, kf_para, 0);       % 卡尔曼滤波结果
[x_ks, ~] = funKalman(z, kf_para, 1);       % 卡尔曼滤波+平滑结果

%% 结果打印
figure;
plot(xreal, 'k.'); hold on;  
plot(z,'bo'); 
plot(x_kf(1,:), 'r*');
plot(x_ks(1,:), 'gd');
h=legend('真实温度', '观测温度', '卡尔曼滤波后的温度', '卡尔曼平滑后的温度', 'Location', 'NorthWest');
set(h,'Fontsize',20);
title('效果对比', 'FontSize', 20);
ylabel('温度', 'FontSize', 20);

fprintf('观测误差均值为：%5.3f；均方差为：%5.3f\n', mean(abs(z-xreal)), std(z-xreal));
fprintf('滤波误差均值为：%5.3f；均方差为：%5.3f\n', mean(abs(x_kf(1,:)-xreal)), std(x_kf(1,:)-xreal));
fprintf('平滑误差均值为：%5.3f；均方差为：%5.3f\n', mean(abs(x_ks(1,:)-xreal)), std(x_ks(1,:)-xreal));