matlab 卡尔曼滤波器PID控制程序。

clc% 清屏

clear all%删拿清除workspace变量

close all%关掉显示图形洞虚窗口

ts=0.001%仿真时间

M=3000%Gen

%Continuous Plant

a=25b=133

sys=tf(b,[1,a,0])%传递函数

dsys=c2d(sys,ts,'z')%离散化

[num,den]=tfdata(dsys,'v')

A1=[0 10 -a]

B1=[0b]

C1=[1 0]

D1=[0]

[A,B,C,D]=c2dm(A1,B1,C1,D1,ts,'z')

Q=1%Covariances of w

R=1%Covariances of v

P=B*Q*B'%Initial error covariance

x=zeros(2,1)%Initial condition on the state

ye=zeros(M,1)

ycov=zeros(M,1)

u_1=0u_2=0

y_1=0y_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

Mn=P*C'/(C*P*C'+R)

P=A*P*A'+B*Q*B'

P=(eye(2)-Mn*C)*P

x=A*x+Mn*(yv(k)-C*A*x)

ye(k)=C*x+D%Filtered value

errcov(k)=C*P*C'%Covariance of estimation error

%Time update

x=A*x+B*u(k)

u_2=u_1u_1=u(k)

y_2=y_1y_1=ye(k)

end

figure(1)

plot(time,yv,'k:',time,y,'r','linewidth'消颤前,2)

xlabel('时间(s)')ylabel('y,yv')

grid on

legend('噪声信号','理想信号')

figure(2)

plot(time,y,'r',time,ye,'k:','linewidth',2)

xlabel('时间(s)')ylabel('y,ye')

grid on

legend('理想信号','滤波信号')

figure(3)

plot(time,errcov,'k','linewidth',2)

grid on

xlabel('时间(s)')ylabel('误差协方差')

我现在也在研究kalman，这是最新发现的一个程序，我做的标注，有问题一块研究

function [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, varargin)

% Kalman filter.

% [x, V, VV, loglik] = kalman_filter(y, A, C, Q, R, init_x, init_V, ...)

%

% INPUTS:

% y(:,t) - the observation at time t 在时间t的观测

% A - the system matrix A 系统矩阵

% C - the observation matrix C 观测矩阵

% Q - the system covariance Q 系统协方差

% R - the observation covariance R 观测协方差

% init_x - the initial state (column) vector init_x 初始状态（列）向量

% init_V - the initial state covariance init_V初始状态协方差拍桐

%

% OPTIONAL INPUTS (string/value pairs [default in brackets]) 选择性输派嫌入（字符串/值 对【默认在括号中】）

% 'model' - model(t)=m means use params from model m at time t [ones(1,T) ] 在时间t，m意味着利用 m模型参数 [ones(1,T) ]

%

% In this case, all the above matrices take an additional final

% dimension, 在这种情况下,上述矩阵采用附加的维数

% i.e., A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m). 例如

% However, init_x and init_V are independent of model(1).

% init_x and init_V相对于模型1是独立的

% 'u' - u(:,t) the control signal at time t [ [] ]

% 在时间t的控制信号

% 'B' - B(:,:,m) the input regression matrix for model m

% 对于模型m的，尘贺手输入回归矩阵

% OUTPUTS (where X is the hidden state being estimated) 输出（其中X是被估计的隐藏状态）

% x(:,t) = E[X(:,t) | y(:,1:t)]

% V(:,:,t) = Cov[X(:,t) | y(:,1:t)]

% VV(:,:,t) = Cov[X(:,t), X(:,t-1) | y(:,1:t)] t >= 2

% loglik = sum{t=1}^T log P(y(:,t))

%

% If an input signal is specified, we also condition on it: 如果一个输入信号是特定的,我们的条件

% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t)]

% If a model sequence is specified, we also condition on it:

% e.g., x(:,t) = E[X(:,t) | y(:,1:t), u(:, 1:t), m(1:t)]

[os T] = size(y)

ss = size(A,1)% size of state space

% set default params

model = ones(1,T)

u = []

B = []

ndx = []

args = varargin

nargs = length(args)

for i=1:2:nargs

switch args{i}

case 'model', model = args{i+1}

case 'u', u = args{i+1}

case 'B', B = args{i+1}

case 'ndx', ndx = args{i+1}

otherwise, error(['unrecognized argument ' args{i}])

end

end

x = zeros(ss, T)

V = zeros(ss, ss, T)

VV = zeros(ss, ss, T)

loglik = 0

for t=1:T

m = model(t)

if t==1

%prevx = init_x(:,m)

%prevV = init_V(:,:,m)

prevx = init_x

prevV = init_V

initial = 1

else

prevx = x(:,t-1)

prevV = V(:,:,t-1)

initial = 0

end

if isempty(u)

[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...

kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, 'initial', initial)

else

if isempty(ndx)

[x(:,t), V(:,:,t), LL, VV(:,:,t)] = ...

kalman_update(A(:,:,m), C(:,:,m), Q(:,:,m), R(:,:,m), y(:,t), prevx, prevV, ...

'initial', initial, 'u', u(:,t), 'B', B(:,:,m))

else

i = ndx{t}

% copy over all elementsonly some will get updated

x(:,t) = prevx

prevP = inv(prevV)

prevPsmall = prevP(i,i)

prevVsmall = inv(prevPsmall)

[x(i,t), smallV, LL, VV(i,i,t)] = ...

kalman_update(A(i,i,m), C(:,i,m), Q(i,i,m), R(:,:,m), y(:,t), prevx(i), prevVsmall, ...

'initial', initial, 'u', u(:,t), 'B', B(i,:,m))

smallP = inv(smallV)

prevP(i,i) = smallP

V(:,:,t) = inv(prevP)

end

end

loglik = loglik + LL

end

---