卡尔曼滤波matlab仿真（可运行）

最新推荐文章于 2024-08-18 10:13:28 发布

try_again_later

最新推荐文章于 2024-08-18 10:13:28 发布

阅读量5.4k

点赞数 4

分类专栏：无人驾驶

本文链接：https://blog.csdn.net/try_again_later/article/details/78304691

版权

一、温度测量

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  
% 程序说明：Kalman滤波用于温度测量的实例(一维）    
function main  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%初始化参数
N=120;  %采样点个数
A=1;  
B=1;  
H=1; 
Q=0.01;  
R=0.25;  
W=sqrt(Q)*randn(1,N);  
V=sqrt(R)*randn(1,N);  
CON=25;  %温度真实值
%分配空间
X=zeros(1,N);    
Z=zeros(1,N); 
Xkf=zeros(1,N);  
P=zeros(1,N);   
X(1)=25.1;  
P(1)=0.01;  
Z(1)=24.9;  
Xkf(1)=Z(1);  
I=eye(1);   
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%kalman过程
for k=2:N  
    X(k)=A*X(k-1)+B*W(k);   
    Z(k)=H*X(k)+V(k);  
    X_pre(k)=A*Xkf(k-1);             
    P_pre(k)=A*P(k-1)*A'+Q;          
    Kg=P_pre(k)/(H*P_pre(k)*H'+R);             
    Xkf(k)=X_pre(k)+Kg*(Z(k)-H*X_pre(k));           
    P(k)=(I-Kg*H)*P_pre(k);  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%误差过程
Err_Messure=zeros(1,N);  
Err_Kalman=zeros(1,N);  
for k=1:N  
    Err_Messure(k)=Z(k)-X(k);  
    Err_Kalman(k)=Xkf(k)-X(k);  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
t=1:N;  
figure;  
plot(t,CON*ones(1,N),&#