%Kalman Estimator: CartKalman.m
close all;%deletes the current figure
clear all;%clear workspace and frees up system memory
clc;%clean screen
N=10;%end time;
T=0.1;%interval;
A=[1 T;0 1];%define the dynamic system
B=[T^2/2;T];%for acceleration
C=[1 0;0 1];%C(1,:)= position output;
Cv=C(2,:);%C(2,:)= velocity output
D=[0;0];%for multi-output
u=[1];%corresponding to position=acceleration control variable
vmn=3;%velocity measurement noise
un=2.0;%acceleration noise/process noise
pmn=1;%position measurement noise
%x(1)=p;%x=state space; x(1)=position
%x(2)=v;%x=state space; x(2)=velocity
Q=un^2*[T^4/4 T^3/2;T^3/2 T^2];%process noise covairance of position
x=[0;0];%initial state of p and v;
xest=x;%initial state of estimate p and v;
%%%%%%%%%%%%%%%Kalman filter%%%%%%%%%%%%%%%%%%%%%%
P=Q;%initial value of estimate covariance position
R=[pmn^2;vmn^2]; %position/velocity measurement error covariance;the covariance with observing noise
Cn=0;%counter
for t=0:T:N;%loop
Cn=Cn+1;%accumulator of iteration number
eXpP=5+3*Cn/2;%expectation of position/velocity
%u=[0.5-0.05*t/1];%acceleration change with time
Pn=un*B*randn;%process noise wk=[w1;w2];
x=A*x+B*u+Pn;%[position;velocity]: x(k+1)=Ax(k)+Bu(k)+w(k)
PMNoise=pmn*randn;%position measurement noise random
VMNoise=vmn*randn;%velocity measurement noise random
MNoise=[PMNoise;VMNoise];%measurement noise zk=[z1;z2];
y=C(1,:)*x+MNoise;%position output: y(k)=Cx(k)+z(k)
yv=C(2,:)*x+MNoise(2,1);
Upd=y-C(1,:)*xest; %update: y_est(k)=y(k)-Cxest(k);Upd(1)=position;Upd(2)=velocity
P1=C(1,:)*P*C(1,:)'+R;%covariance corresponding to the estimate (Position/velocity)
Kg=A*P*C(1,:)'/P1;%Kalman gain
xest=A*xest+B*u+Kg*Upd;%update estimate of system state:
P=A*P*A'+Q-A*P*C(1,:)'*inv(P1(1))*C(1,:)*P*A;%update estimate of system state
pt(Cn)=x(1);%Expected position
vt(Cn)=x(2);%Expected velocity
pe(Cn)=xest(1);%estimate position
ve(Cn)=xest(2);%estimate velocity
pm(Cn)=y(1);%measurement position
vm(Cn)=yv(1);%measurement velocity
end;
pr=pt-pe;%expected value - estimated value
vr=vt-ve;%expected value - estimated value
close all;%deletes the current figure
t=(0:T:N)';%get the time axis10
%velocity estimate
figure(1);%velocity
set(gca,'FontSize',16);
get(gca,'default');
plot(t,vm,'k');%Black=Velocity measurement;
hold on;
plot(t,ve,'r','LineWidth',3);%red=kalman estimate
plot(t,vt,'--*b','MarkerSize',4);%Blue=Velocity true;
legend('Measure','KalmanEstimate','ExpValue',2);
xlabel('Time (s)','FontWeight','bold','FontSize',14);
ylabel('Velocity (m/s)','FontWeight','bold','FontSize',14);
%axis([0 N 30 60]);
%title('Kalman predictor of velocity');
%position estimate
figure(2);%position
set(gca,'FontSize',16);
get(gca,'default');
plot(t,pm,'k');%Black=position measurement;
hold on;
plot(t,pe,'r','LineWidth',3);%red=Kalman estimate
plot(t,pt,'--*b','MarkerSize',4);%Blue=Velocity true;
legend('Measure','KalmanEstimate','ExpValue',2);
xlabel('Time (s)','FontWeight','bold','FontSize',14);
ylabel('Position (m)','FontWeight','bold','FontSize',14);
%axis([0 N 30 60]);
%title('Kalman predictor of position');
%error between expected and estimated values
figure(3);%error
set(gca,'FontSize',16);
get(gca,'default');
plot(t,pr,'k');%Black=position measurement;
hold on;
plot(t,vr,'--*b','MarkerSize',4);%Blue=Velocity true;
legend('Postion error','Velocity error',2);
xlabel('Time (s)','FontWeight','bold','FontSize',14);
ylabel('Error (m)','FontWeight','bold','FontSize',14);
%axis([0 N 30 60]);
%title('Kalman predictor of position');
%??????????????????????????-
%??????????????????????????-
%Gradient descent optimization of the optimal cart control: CostFunction.m
%close all;%deletes the current figure
clear all;%clear workspace and frees up system memory
clc;%clean screen
q=0.01;%coefficient
r=10;%coefficient
eps=0.0005; % gradient descent step size
te=10; %end time
T=te/100; %time step size or interval
t=0:T:te;
N=size(t,2);
w=99;
%Configure the dynamic system.
A=[1 T; 0 1];%system state matrix
B=[T^2/2; T];%system input matrix11
C=[1 0; 0 0];%system output matrix
D=[0;0];%
sys=ss(A,B,C,D);%system structure
%===
u=zeros(1,N);%Guess an initial optimal control.
SizeHp=inf;
for Cn=1:N;%iteration number
[x,t]=lsim(sys,u,t);%Simulate the system with the given control.
Lambda=zeros(N,2);%Compute the co-state.
Lambda(N,1)=2*q*(x(N,1)-w);
Lambda(N,2)=0;
for i=N-1:-1:1;%
LambdaDot=[0 -Lambda(i+1,1)];
Lambda(i,:)=Lambda(i+1,:)-T*LambdaDot;
end;
for i=1:N;%Compute the Hamiltonian.
H(i)=r*u(i)*u(i)+Lambda(i,:)*[x(i,2);u(i)];
end;
for i=1:N;%Compute the partial of the Hamiltonian with respect to the control.
Hp(i)=2*r*u(i)+Lambda(i,2);
end;
J=q*(x(N,1)-w)^2+r*T*(u(1)^2+u(N)^2)/2;%Compute the cost function.
J=J+r*T*norm(u(2:N-1))^2;
JArr(Cn)=J;%update each JArr in vectors
SizeHpOld=SizeHp;%Compute the size of Hp.
SizeHp=norm(Hp);
if (SizeHp>SizeHpOld) break; %Quit when Hp gets close to zero.
end;
SizeHpArr(Cn)=SizeHp;%update each SizeHp in vectors
if (SizeHp/N<0.01) break; %Quit when Hp gets close to zero.
end;
u=u-eps*Hp;%Update the control using gradient descent.
end;
%===figure 1: system response===
figure(4);
set(gca,'FontSize',14);
get(gca,'default');
plot(t,x,'k',...
'LineWidth',1.5,...
'MarkerEdgeColor','k',...
'MarkerFaceColor','g',...
'MarkerSize',4);
xlabel('Time','FontWeight','bold','FontSize',16);
ylabel('Position response','FontWeight','bold','FontSize',16);
%===figure 2: cost function===
figure(5);
set(gca,'FontSize',14);
get(gca,'default');
plot(JArr,'--*k',...
'LineWidth',1.5,...
'MarkerEdgeColor','k',...
'MarkerFaceColor','g',...
'MarkerSize',6);
%title('Cost Function');
text(15,90,['Cost=',num2str(J)],'FontSize',16);
xlabel('Iteration Number','FontWeight','bold','FontSize',16);
ylabel('Cost function value-J','FontWeight','bold','FontSize',16);
%===figure 3: control output===12
figure(6);
set(gca,'FontSize',14);
get(gca,'default');
plot(t,u,'--*k',...
'LineWidth',1.5,...
'MarkerEdgeColor','k',...
'MarkerFaceColor','g',...
'MarkerSize',4);
%title('Optimal Control');
xlabel('Time','FontWeight','bold','FontSize',16);
ylabel('Control output','FontWeight','bold','FontSize',16);
%===figure 4: lambda, Hamiltonian, Hamilton partial differential===
figure(7);
ax1=gca;
%grid on;
set(ax1,'XColor','k','YColor','k','FontSize',14);
h1=line(t,Lambda,'Color','k','LineStyle','--');%
xlabel('Time','FontWeight','bold','FontSize',16);
ylabel('Coefficient \lambda','FontWeight','bold','FontSize',16);
legend('\lambda 1','\lambda 2',2);
%axis([0 t -1.4 1]);
ax2=axes('Position',get(ax1,'Position'),...
'YAxisLocation','right',...
'Color','none',...
'FontSize',14,...
'XColor','k','YColor','k');
h2=line(t,H,'Color','b','Parent',ax2,'LineWidth',1.5);%Hamiltonian
h3=line(t,Hp,'Color','r','Parent',ax2,'LineWidth',1.5,'Marker','+');%H'
ylabel('Hamiltonian','FontWeight','bold','FontSize',14);
legend('H','Hp',1);
%??????????????????????????
%??????????????????????????
%system response: SystemResponse.m
clear;
clc;
%close all;
T=0.1;
A=[1 T;0 1];%system state matrix
B=[T^2/2;T];%system input matrix
C=[1 0];%system output matrix
D=[0];%
sys=ss(A,B,C,D);%Specify state-space models or convert LTI model to state space
K=lqry(sys,1,10);%design LQ-optimal gain K:u=-Kx minimizes J(u);linearquadratic (LQ) state-feedback regulator with output weighting
P=sys(:,[1 1]);%Seperate control input u and disturbance input w(process noise);
Kest=kalman(P,1,0.1);%Design discrete-time Kalman state estimator Kest
评论0