卡尔曼滤波

卡尔曼滤波的一个小例子，原贴在：

http://blog.csdn.net/datase/article/details/78508752

我认为他的代码有几个小问题：

1.反馈量只有位移。我把输出量改成了位移，速度和加速度三者的列向量。

2.我感觉原代码的白噪声的生成看起来有点奇怪，我这边用了wt = sqrt(4) * randn(1,N);  生成方差为4，均值为0的白噪声。

3.他在卡尔曼的五条方程里对K和K+1写的有问题，我吧那边也改了。

4.因为输出量是三维的，因此Rk就从常数变成了3*3的协方差矩阵。

代码如下——

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
    %     设置初始化信息  
    %  N：设置卡尔曼滤波器追踪点数  
    %  r：设置估计变量个数，这里r=3  
    %  s：被追踪的火箭的距离，初始值为1000m  
    %  v：火箭的速度，初始值为50m/s  
    %  a：火箭的加速度，初始值为20m/s2，此时加速度默认为不变  
    %  T: 雷达的扫描间隔，此时设为1秒  
    %  wt: 系统噪声，方差为20  
    %  vt: 量测噪声，方差为16  
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
    clear all;  
    close all;  
    N = 100;  
    r = 3;  
    t = 1:1:N;  
    T = 1;  
    s = zeros(1,N);  
    v = zeros(1,N);  
    a = zeros(1,N);  
    a(t) = 20;  
    s0 = 1000;  
    v0 = 50;  
    for n = 1:N  
        v(n) = v0 + a(n)*n;  
        s(n) = 1000+v0*n+0.5*a(n)*n*n;  
    end  
    wt = sqrt(4) * randn(1,N);    
    wat  = sqrt(4) * rand(1,N);%生成方差为4的噪声
    wst  = sqrt(400) * rand(1,N);
    wvt  = sqrt(49) * rand(1,N);
    s = s + wt%实际;
    v = v + wt;
    a = a + wt;
    swt = s + wst;%传感器测量  
    vwt = v + wvt;  
    awt = a + wat;  
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
    % 卡尔曼滤波部分，继承之前初始化变量  
    % A：转移矩阵  
    % H：量测矩阵  
    % Qk：系统噪声矩阵  
    % Rk：量测噪声矩阵  
    % P0：均方误差矩阵初始值  
    % Y：火箭的状态矩阵，由k_s，k_v,k_a组成  
    % Y0：状态矩阵的初始值  
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
    Y0 = [900 0 0]'; 
    M = zeros(r,N);
    M(1,:) = swt;
    M(2,:) = vwt;
    M(3,:) = awt;
    
    Y = [Y0 zeros(r,N-1)];  
    A = [1 1 0;0 1 1;0 0 1];  
    H = [1 0 0;0 1 0;0 0 1];  
    Qk = [4 0 0;0 4 0;0 0 4];  
    Rk = [400 0 0;0 49 0;0 0 4];  
    P0 = [30 0 0;0 20 0;0 0 10];  
    P1 = P0;  
    P2 = zeros(r,r);  
    for k = 1:N-1  
        Y(:,k+1) = A*Y(:,k);  
        P2 = A*P1*A'+Qk;  
        Kk = P2*H'*inv(H*P2*H'+Rk);  
        Y(:,k+1) = Y(:,k+1)+Kk*(M(:,k+1)-H*Y(:,k+1)); %测量 - 先验数据 
        P1 = (eye(r,r)-Kk*H)*P2;  
    end  
    k_s = Y(1,:);  
    k_v = Y(2,:);  
    k_a = Y(3,:);  
    subplot(3,1,1);  
    plot(t,s(t),'-',t,k_s(t),'o');  
    title('距离');  
    legend('实际值','估计值');  
    xlabel('t');  
    ylabel('s');  
    subplot(3,1,2);  
    plot(t,v(t),t,k_v(t),'+');  
    title('速度');  
    legend('实际值','估计值');  
    xlabel('t');  
    ylabel('v');  
    subplot(3,1,3);  
    plot(t,a(t),t,k_a(t),'-x');  
    title('加速度');  
    legend('实际值','估计值');  
    xlabel('t');  
    ylabel('a');  
    axis([0,N,0,30]);