卡尔曼滤波算法和扩展卡尔曼滤波算法

卡尔曼滤波算法和扩展卡尔曼滤波算法，完整的matlab程序和仿真结果，程序简单，易懂。

%对一个控制系统:状态方程:
    %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）为高斯白噪声
%下面我先构造一组测量数据然后根据测量数据估计状态变量的值%
%问题是:为什么估计的数值总随着初始状态的值X（0）变化而变化并没有看到收敛.....
%构造测量数据%
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）
%估计状态变量的值%
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