Matlab--基于卡尔曼滤波器的PID控制

最新推荐文章于 2024-07-22 11:01:43 发布

她和理想啊

最新推荐文章于 2024-07-22 11:01:43 发布

阅读量6.4k

点赞数 9

本文链接：https://blog.csdn.net/jiasuizhou6733/article/details/88919989

版权

Matlab 专栏收录该内容

13 篇文章 5 订阅

订阅专栏

在现代随机最优控制和随机信号处理技术中，信号和噪声往往是多维非平稳随机过程。因其时变性，功率谱不固定。在1960年年初提出了卡尔曼滤波理论，该理论采用时域上的递推算法在数字计算机上进行数据滤波处理。
对于离散域线性系统
$x (k) = A x (k - 1) + B (u (k) + w (k))$
$y_{v}=Cx(k)+v(k)$
式中，w（k）为过程噪声信号，v（k）为测量噪声信号。
离散卡尔曼滤波器递推算法为：
$状态预测方程： x (k) = A x (k - 1) + B (u (k)$
$协方差预测方程：P(k)=AP(K-1)A^{T}+BQB^{T}$

$卡尔曼滤波增益：M_{n}=\displaystyle\frac {P(k)C^{T}}{CP(k)C^{T}+R}$
$最优值更新方程：x(k)=Ax(k-1)+M_{n}(k)(y_{v}(k)-CAx(k-1))$
$协方差更新方程：P(k)=(I_{n}-M_{n}(k)C)P(k)$
$y_{e}(k)=Cx(k)$
卡尔曼滤波器结构图：
By_ZC
实验对象为二阶传递函数：
$G_{p}(s)=\displaystyle\frac {133}{s^{2}+25s}$
取采样时间为1ms,采用Z变换将对象离散化，并描述为离散化状态方程为：
$x (k + 1) = A x (k) + B (u (k) + w (k))$
$y (k) = C x (k)$
带有测量噪声的输出；
$y_{k}=Cx(k)$

%Kalman filter
%x=Ax+B(u+w(k));
%y=Cx+D+v(k)
clear all;
close all;

ts=0.001;
M=3000;

%Continuous Plant  连续系统
a=25;b=133;
sys=tf(b,[1,a,0]);        %构造连续系统的模型
dsys=c2d(sys,ts,'z');     %将传递函数离散化
[num,den]=tfdata(dsys,'v');%得到离散模型的分子的分子分母

A1=[0 1;0 -a];
B1=[0;b];
C1=[1 0];
D1=[0];
[A,B,C,D]=c2dm(A1,B1,C1,D1,ts,'z');%%将连续的LTI的系统转变成离散的时间系统

Q=1;           %Covariances of w     测量噪声Q
R=1;           %Covariances of v     输入噪声R

P=B*Q*B';          %Initial error covariance  初始协方差误差
x=zeros(2,1);      %Initial condition on the state  状态的初始条件

ye=zeros(M,1); %%储存的y值
ycov=zeros(M,1);

u_1=0;u_2=0;
y_1=0;y_2=0;

for k=1:1:M
time(k)=k*ts;

w(k)=0.10*rands(1);   %Process noise on u      噪声
v(k)=0.10*rands(1);   %Measurement noise on y   噪声

u(k)=1.0*sin(2*pi*1.5*k*ts);
u(k)=u(k)+w(k);                % 输入加上噪声
  
y(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2;
yv(k)=y(k)+v(k);% 输出加上噪声

%Measurement update
   
   %Time update
    x=A*x+B*u(k);               %状态预测方程
    P=A*P*A'+B*Q*B';            %协方差预测方程
    
    %Measurement update
    Mn=P*C'/(C*P*C'+R);         %卡尔曼增益的计算
    x=A*x+Mn*(yv(k)-C*A*x);     %最优值的计算
    P=(eye(2)-Mn*C)*P;          %协方差更新方程
    
    errcov(k)=C*P*C';        %  Covariance of estimation error  估计误差的协方差
      ye(k)=C*x+D;           %Filtered value   滤波以后的值
      
	 u_2=u_1;u_1=u(k);
  	 y_2=y_1;y_1=ye(k);
end
figure(1);
plot(time,y,'r',time,yv,'k:','linewidth',2);
xlabel('time(s)');ylabel('y,yv')
legend('ideal signal','signal with noise');

figure(2);
plot(time,y,'r',time,ye,'k:','linewidth',2);
xlabel('time(s)');ylabel('y,ye')
legend('ideal signal','filtered signal');

figure(3);
plot(time,errcov,'k','linewidth',2);
xlabel('time(s)');ylabel('Covariance of estimation error');