利用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.后验估计,理论值 和 真实值
通过比较估计值和真实值可以看出精度还是不错的