matlab:卡尔曼滤波示例

kalman滤波基础及matlab仿真程序，王可东，例5-3，P127

%卡尔曼滤波示例
clc;clear all;
T = 0.2;
Phi = [1 T T*T/2; 0 1 T; 0 0 1];    %状态转移矩阵
G = [0 0 T]';       %伽马，量测噪声矩阵
H = [1 0 0];        %量测矩阵
Q= 0.1; %量测噪声方差
R= 0.04; %观测噪声方差
I = eye(3); %初值
xr(:,1) = randn(3,1);   xe(:,1) = zeros(3,1);
Ppos = eye(3); 
Ppre(:,1) = diag(Ppos);     %一步预测协方差矩阵的对角线
Pest(:,1) = diag(Ppos);     %估计偏差协方差矩阵的对角线
 
for k = 2:100
    %方程1——一步预测估计
    x(:,k) = Phi * xe(:,k-1);       %一步预测估计的状态
    %方程2——一步预测协方差矩阵
    Pneg = Phi * Ppos *Phi' +G * Q * G' ;       %一步预测协方差矩阵
    Ppre(:,k) = diag(Pneg);         %取出对角线元素
    w = Q^0.5 * randn;              %量测噪声
    xr(:,k) = Phi*xr(:,k-1) + G*w;  %x_k
    v =  R^0.5 * randn;              %观测噪声
    z(:,k) = H*xr(:,k) + v;         %z_k
    %方程3——系数矩阵更新
    K(:,k) = Pneg * H' *inv(H * Pneg * H' +R);      %系数矩阵
    %方程4——量测滤波修正
    xe(:,k) = x(:,k) +  K(:,k)* (z(:,k) - H * x(:,k));
    Ppos = (I -K(:,k) *H) *Pneg*(I -K(:,k) *H)' + K(:,k)*R * K(:,k)'; %估计偏差协方差矩阵
    Pest(:,k) = diag(Ppos); 
end

Ks = Ppos * H' *(1/R);      %增益矩阵
xe1(:,1) = zeros(3,1);

%量测修正状态估计
for k = 2:100
    xe1(:,k) = Phi* xe1(:,k-1) + Ks * (z(:,k)- H * Phi * xe1(:,k-1))   
end
t= T*(1:100);
figure(1)
plot(t, abs(x(1,:) - xr(1,:)),'k', t, abs(xe(1,:) - xr(1,:)),'ro-', t, abs(xe1(1,:) - xr(1,:)),'b*-');
legend('预测值误差', '滤波误差', '稳态增益滤波误差');
xlabel('时间');ylabel('位移估计误差');  

figure(1)       %位置
plot(t, abs(x(1,:) - xr(1,:)),'k', t, abs(xe(1,:) - xr(1,:)),'ro-', t, abs(xe1(1,:) - xr(1,:)),'b*-');
legend('预测值误差', '滤波误差', '稳态增益滤波误差');
xlabel('时间');ylabel('位移估计误差');  

figure(2)        %速度
plot(t, abs(x(2,:) - xr(2,:)),'k', t, abs(xe(2,:) - xr(2,:)),'ro-', t, abs(xe1(2,:) - xr(2,:)),'b*-');
legend('预测值误差', '滤波误差', '稳态增益滤波误差');
xlabel('时间');ylabel('速度估计误差');  

figure(3)        %加速度
plot(t, abs(x(3,:) - xr(3,:)),'k', t, abs(xe(3,:) - xr(3,:)),'ro-', t, abs(xe1(3,:) - xr(3,:)),'b*-');
legend('预测值误差', '滤波误差', '稳态增益滤波误差');
xlabel('时间');ylabel('加速度估计误差');

运行结果如下：