首页 > 代码库 > 卡尔曼滤波学习笔记1-Matlab模拟温度例子--代码比较乱,还需优化

卡尔曼滤波学习笔记1-Matlab模拟温度例子--代码比较乱,还需优化

 

温度模拟参数选取

xk系统状态实际温度
A系统矩阵温度不变,为1
B、uk状态的控制量无控制量,为0
Zk观测值温度计读数
H观测矩阵直接读出,为1
wk过程噪声温度变化偏差,常量1e-1
vk测量噪声读数误差,常量1e-6

clc;
clear all;
close all;

N = 200; % 迭代次数
%w(1)=0;
%w=randn(1,N);
W = 0; %系统控制矩阵
x(1) = 0;
A = 1; %温度模拟A为1
V = rand(1,N);
q1 = std(V);
%Rvv=q1.^2;
Rvv = 0.1;
q2 = std(x);
Rxx = q2.^2;
q3 = std(W);
%Rww=q3.^2;
Rww = 0.00001; %温度模拟Q为1e-6
%c=0.6;
c=1; %温度模拟H为1
for k = 1:N
Y(k) = 25 + sqrt(0.1)*rand(1);%温度模拟平均温度为25度 方差(协方差)为0.1的温度输入 测量方程,其中V为测量系统的噪声,c为测量系统的参数
end
p(1)=10; %协方差 初始值
s(1)=1; %最优估计 初始值
for t = 2:N
s(t) = A*s(t-1) + W;
p1(t) = A.^2*p(t-1) + Rww;
%%协方差估计 求当前时刻的估计值的偏差,a为系统参数,没有控制量,所以没有参数b,Rww为噪声
b(t) = c * p1(t) /(c.^2*p1(t) +Rvv);
s(t) = A *s(t) + b(t)*(Y(t) -A*c*s(t));
p(t) = p1(t) -c*b(t)*p1(t);

end
figure
plot(Y,‘g--‘);
hold on
plot(s,‘r--‘);

 

来源于:http://blog.csdn.net/s597471018/article/details/8275454

卡尔曼滤波学习笔记1-Matlab模拟温度例子--代码比较乱,还需优化