卡尔曼滤波学习笔记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模拟温度例子--代码比较乱，还需优化

最新文章

热门文章