%對一個控制系統(tǒng):狀態(tài)方程:
????%Ed（i+1）=t*（Ed（i）+Rs（i）*Eq（i）+x（i）*Ed（i）+Wr（i）*Eq（i）+T（i）*Rs（i））+Ed（i）+w（i）;
????%Eq（i+1）=t*（Eq（i）+x（i）*Eq（i）+Rs（i）*Ed（i）+Wr（i）*Ed（i）+T（i）*x（i））+Eq（i）*T（i）+w（i）;
????%Wr（i+1）=t*（Wr（i）+Rs（i）*Wr（i）+x（i）*Ed（i）+x（i）*Eq（i）+Wr（i）*Ed（i））+Wr（i）+w（i）;
????%Rs（i+1）=Rs（i）+w（i）;
????%x（i+1）=x（i）+w（i）;
????%T（i+1）=T（i）+w（i）;
%測量方程
????%ID（i）=Ed（i）+v（i）;
????%IQ（i）=Eq（i）+v（i）;
%w（i）v（i）為高斯白噪聲
%下面我先構(gòu)造一組測量數(shù)據(jù)然后根據(jù)測量數(shù)據(jù)估計狀態(tài)變量的值%
%問題是:為什么估計的數(shù)值總隨著初始狀態(tài)的值X（0）變化而變化并沒有看到收斂.....
%構(gòu)造測量數(shù)據(jù)%
t=0.01;
Ed（1）=0;
Eq（1）=0;
Wr（1）=2;
Rs（1）=0.02;
x（1）=0.4;
T（1）=1;
Q=1e-10;
R=1e-10;
n=100;
m=6;
p=2;
w=random（‘norm‘0Qmn）;
v=random（‘norm‘0Rpn）;
x_true=zeros（6n+1）;
y_true=zeros（2n+1）;
x_true（:1）=[Ed（1）;Eq（1）;Wr（1）;Rs（1）;x（1）;T（1）];
for?i=1:n
????Ed（i+1）=t*（Ed（i）+Rs（i）*Eq（i）+x（i）*Ed（i）+Wr（i）*Eq（i）+T（i）*Rs（i））+Ed（i）+w（1i）;
????Eq（i+1）=t*（Eq（i）+x（i）*Eq（i）+Rs（i）*Ed（i）+Wr（i）*Ed（i）+T（i）*x（i））+Eq（i）*T（i）+w（2i）;
????Wr（i+1）=t*（Wr（i）+Rs（i）*Wr（i）+x（i）*Ed（i）+x（i）*Eq（i）+Wr（i）*Ed（i））+Wr（i）+w（3i）;
????Rs（i+1）=Rs（i）+w（4i）;
????x（i+1）=x（i）+w（5i）;
????T（i+1）=T（i）+w（6i）;
????ID（i）=Ed（i）+v（1i）;
????IQ（i）=Eq（i）+v（2i）;
????x_true（:i+1）=[Ed（i+1）;Eq（i+1）;Wr（i+1）;Rs（i+1）;x（i+1）;T（i+1）];
????y_true（:i）=[ID（i）;IQ（i）];
end
%?HZB=0.00:0.01:1.00;
%?plot（HZBx）
%估計狀態(tài)變量的值%
syms?Ed?Eq?Wr?Rs?x?T;
t=0.01;
F=[t*（Ed+Rs*Eq+x*Ed+Wr*Eq+T*Rs）+Ed?t*（Eq+x*Eq+Rs*Ed+Wr*Ed+T*x）+Eq?t*（Wr+Rs*Wr+x*Ed+x*Eq+Wr*Ed）+Wr?Rs?x?T];
h=[Ed?Eq];
X=[Ed?Eq?Wr?Rs?x?T];
FF=jacobian（FX）;
hh=jacobian（hX）;
N=100;
HZB=0.00:0.01:1;
X1=[0?0?2?0.0154?0.3787?0.99]‘;
Po=0.0001*eye（6）;
Z=[ID‘?IQ‘];
Q=1e-10;
R=1e-10;
x_filter=zeros（6N+1）;
cova_filter=zeros（6N+1）;
x_filter（:1）=[0?0?2?0.0154?0.3787?0.99]‘;
cova_filter（:1）=diag（Po）;
for?i=1:N
????X=X1‘;
????Ed=X（1）;
????Eq=X（2）;
????Wr=X（3）;
????Rs=X（4）;
????x=X（5）;
????T=X（6）;
????P=eval（FF）*Po*（eval（FF））‘+Q*eye（6）;
????X=eval（F）;
????Ed=X（1）;
????Eq=X（2）;
????Wr=X（3）;
????Rs=X（4）;
????x=X（5）;
????T=X（6）;
????K=P*eval（hh）‘*inv（eval（hh）*P*eval（hh）‘+R）;
????P1=（eye（6）-K*eval（hh））*P;
%?????P1=inv（P+（eval（hh））‘*inv（R）*eval（hh））;
%?????K=P1*（eval（hh））‘*inv（R）;
????X1=X‘+K*[Z（i:）‘-（eval（h））‘];
????Po=P1;
????P1;
????x_filter（:i+1）=X1;
????cova_filter（:i+1）=diag（P1）;
end
%?????X‘
figure（1）
plot（x_true（1:）‘r‘）
hold?on
plot（x_filter（1:）‘k‘）;
figure（2）
plot（x_true（2:）‘r‘）
hold?on
plot（x_filter（2:）‘k‘）;
figure（3）
plot（x_true（3:）‘r‘）
hold?on
plot（x_filter（3:）‘k‘）;
figure（4）
plot（x_true（4:）‘r‘）
hold?on
plot（x_filter（4:）‘k‘）;
figure（5）
plot（x_true（5:）‘r‘）
hold?on
plot（x_filter（5:）‘k‘）;
figure（6）
plot（x_true（6:）‘r‘）
hold?on
plot（x_filter（6:）‘k‘）;

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????2793??2006-01-19?12:00??bjskalman.m

?????文件???????5996??2006-02-10?11:14??EEkalman.m

-----------?---------??----------?-----??----

?????????????????8789????????????????????2