MATLAB--卡尔曼滤波

 
利用matlab来实现卡尔曼滤波，一个简单的例子：

clear all;clc;close all;

%*************利用卡尔曼滤波算法来估计状态值**************%

%定义状态转移矩阵和观测矩阵
syms x u;
h(x) = x;  %exp(x^3+2*x+2);
H_ = diff(h,x);

f(x,u) = x + u;
F_ = diff(f,x);

%%首先构造一些观测值
u_k = 0.015;
x_ = 0.01:u_k:1;
Q = 0.01;
R = 0.25;
nosie_Q = sqrt(Q)*randn(1,size(x_,2));%产生高斯噪音
nosie_R = sqrt(R)*randn(1,size(x_,2));%产生高斯噪音

x_data = f(x_,0)+nosie_Q;
y_ = h(x_data) + nosie_R;%构造一系列观测值

%初始化
x_posterior = 0.01;
P_posterior = 0.04;
x_prior = 0;
P_prior = 0;
K = 0;

H = H_;
F = F_;
x_posterior_save = [];
P_posterior_save = [];

%卡尔曼滤波过程
for i = 1:size(x_,2)
    %第一步：预测
    x_prior = f(x_posterior,u_k);%prior先验，posterior后验
    P_prior = F(x_posterior,u_k)*P_posterior*F(x_posterior,u_k)' + Q;
    
    %第二步：更新
    H_vpa = vpa(H(x_prior),5);
        %卡尔曼增益
        K = P_prior*H_vpa/(H_vpa*P_prior*H_vpa'+ R);
        K = vpa(K,5);
        %后验方差
        P_posterior = (1-K*H_vpa)*P_prior;
        P_posterior = vpa(P_posterior,5);
        %后验估计
        x_posterior = x_prior + K*(y_(i)-h(x_prior));
        x_posterior = vpa(x_posterior,5);
        %保存数据
        x_posterior_save = [x_posterior_save x_posterior];
        P_posterior_save = [P_posterior_save P_posterior];
        
    fprintf('后验x为：%f\n',x_posterior);
end

%不经优化直接求解的数值，用于比较
x_pre = [];
for i = 1:size(y_,2)
    x_pre_ = solve(h==y_(i));
    x_pre = [x_pre x_pre_(1)];
    x_pre = vpa(x_pre,4);
end

figure('Name','Y');
%axis([0 1 0 120]);
hold on;
grid on;
plot(x_,y_,'black');
plot(x_,h(x_),'red');
legend('观测值','理论值');  

figure('Name','x');
%axis([0 100 -30 140]);
hold on;
grid on;
plot(1:u_k*100:100,x_posterior_save*100,'red',1:u_k*100:100,1:u_k*100:100,'green',1:u_k*100:100,x_data.*100,'blue');
plot(1:u_k*100:100,x_pre*100,'-k');
legend('卡尔曼优化值','理论值','真实值','未优化值');  

结果：

1.创建的数据以及理论曲线：

 

2.后验估计，理论值 和 真实值

通过比较估计值和真实值可以看出精度还是不错的