卡尔曼滤波算法---基于matlab实现

周亚龙 · 发表于 2018-3-18 23:13:26

卡尔曼滤波实现简单，滤波效果好，下面分享一个基于卡尔曼滤波的matlab算法，数据全部为一维状态，本人弥补的详细备注，供爱好者研究学习。%%%%%%%%%%%%%%%%%%
%功能说明：Kalman滤波用在一维温度数据测量系统中
function main
%%%%%%
N = 120;                                  %一共采样的点数，时间单位是分钟，可理解为实验进行了60分钟的测量
CON = 25;                               %室内温度的理论值，在这个理论值的基础上受过程噪声会有波动
%ones(a,b) 产生a行b列值为1的矩阵
%zeros(a,b)产生a行b列值为0的矩阵
Xexpect = CON*ones(1,N);    %期望的温度是恒定的25度，但实际温度不可能会这样的

X = zeros(1,N);                      %房间各时刻真实温度值
Xkf = zeros(1,N);                   %Kalman滤波处理的状态，也叫估计值
Z = zeros(1,N);                      %温度计测量值
P = zeros(1,N);
%X(1)为数组的第一个元素
X(1) = 25.1;    %假如初始值房间温度为25.1度
P(1) = 0.01;    %初始值的协方差（测量值 - 真实值）^2

%产生0-1的随机数  模拟系统的随机数据
Z = 24+rand(1,N)*10 - 5;

Z(1) = 24.9;    %温度计测的第一个数据
Xkf(1) = Z(1);  %初始测量值为24.9度，可以作为滤波器的初始估计状态噪声

Q = 0;       %扰动误差方差（不存在扰动误差只有测量误差）
R = 0.25;       %测量误差方差
%sqrt(Q)*randn(1,N)为产生方差为0.01的随机信号
%W为过程噪声
%V为测量噪声
W = sqrt(Q)*randn(1,N); %方差决定噪声的大小
V = sqrt(R)*randn(1,N);%方差决定噪声的大小
%系统矩阵
%解释：因为该系统的数据都是一维的，故各变换矩阵都是1，原因自己找书理解
F = 1;
G = 1;
H = 1;
%eye产生m×n的单位矩阵数值应该为1
I = eye(1);  %本系统状态为一维
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%模拟房间温度和测量过程，并滤波

for k = 2:N
%第一步：随时间推移，房间真实温度波动变化
%k时刻房间的真实温度，对于温度计来说，这个真实值是不知道的但是它的存在又是客观事实，读者要深刻领悟这个计算机模拟过程
X(k) = F*X(k - 1)+G*W(k - 1);%实际值为理想值叠加上扰动噪声
%第二步：随时间推移，获取实时数据
%温度计对K时刻房间温度的测量，Kalman滤波是站在温度计角度进行的，
%它不知道此刻的真实状态X(k),只能利用本次测量值Z(k)和上一次估计值Xkf(k)来做处理，其目标是最大限度地降低测量噪声R的影响
%尽可能的逼近X(k),这也是Kalman滤波目的所在

%修改本次函数
%Z(k) = H*X(k)+V(k);             %测量值为实际值叠加上测量噪声

%第三步：Kalman滤波
%有了k时刻的观测Z(k)和k-1时刻的状态，那么就可以进行滤波了，
%读者可以对照（3.36）到式（3.40），理解滤波过程
X_pre = F*Xkf(k - 1);                      %状态预测       X_pre为上一次卡尔曼滤波值
P_pre = F*P(k - 1)*F + Q;             %协方差预测
%inv()为求一个方阵的逆矩阵。
Kg = P_pre*inv(H * P_pre*H' + R);  %计算卡尔曼增益
e = Z(k) - H*X_pre;                         %新息                本次测量值与上次预测值之差
Xkf(k) = X_pre + Kg*e;                   %状态更新       本次预测值
P(k) = (I - Kg*H)*P_pre;                   %协方差更新
end
%计算误差
Err_Messure = zeros(1,N); %测量值与真实值之间的偏差
Err_Kalman = zeros(1,N);    %Kalman估计与真实值之间的偏差
for k = 1:N
Err_Messure(k) = abs(Z(k) - X(k)); %abs()为求解绝对值
Err_Kalman(k) = abs(Xkf(k) - X(k));
end
t = 1:N;
figure %画图显示
%依次输出理论值，叠加过程噪声（受波动影响）的真实值
%温度计测量值，Kalman估计值
plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');
legend('期望值','真实值','观测值','Kalman滤波值');
xlabel('采样时间/s');
ylabel('温度值/C');
%误差分析图
figure  %画图显示
plot(t,Err_Messure,'-b.',t,Err_Kalman,'-k*');
legend('测量偏差','Kalman滤波偏差');
xlabel('采样时间/s');
ylabel('温度偏差值/C');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%