授之以渔：卡尔曼滤波器 ....大泻蜜 .........................

1万 · 2011-12-8 18:15

继续听highgear老师上课~~~

只看该作者 · 2011-12-8 21:46

第一个例子中的代码

 

Ts = 1;                    采样时间

s = tf('s');

sysc = 100/(60*s +1);      真实系统的传递函数

sysd = c2d(sysc, Ts);



t = 0:TsTs*300);

u = ones(1, length(t));     系统输入

ys = lsim(sysd, u, t, 0);   系统输出

ys = ys + 10*rand(size(ys));  测量量 = 系统输出 + 噪声



A = [1, 0; 0, 1];

B = [1; 0];

C = [1, 0];

N = length(A);          系统维数

Len = length(u);



yout = zeros(Len, 1);   滤波器输出

Xh = zeros(N, 1);       状态变量

P = eye(N);              

Q = 2 * eye(N);         系统噪声

R = 50;                 测量噪声

for i = 1 : Len

    Xh = A*Xh + B*u(i);

    P = A*P*A' + Q;

    K = P*C'* inv(C*P*C' + R);

    Xh = Xh + K*(ys(i) - C*Xh);

    P = P - K*C*P;

    yout(i) = C*Xh;

end

Ts = 1;                    采样时间

s = tf('s');

sysc = 100/(60*s +1);      真实系统的传递函数

sysd = c2d(sysc, Ts);

 

t = 0:Ts:(Ts*300);

u = ones(1, length(t));     系统输入

ys = lsim(sysd, u, t, 0);   系统输出

ys = ys + 10*rand(size(ys));  测量量 = 系统输出 + 噪声

 

A = [1, 0; 0, 1];

B = [1; 0];

C = [1, 0];

N = length(A);          系统维数

Len = length(u);

 

yout = zeros(Len, 1);   滤波器输出

Xh = zeros(N, 1);       状态变量

P = eye(N);              

Q = 2 * eye(N);         系统噪声

R = 50;                 测量噪声

for i = 1 : Len

    Xh = A*Xh + B*u(i);

    P = A*P*A' + Q;

    K = P*C'* inv(C*P*C' + R);

    Xh = Xh + K*(ys(i) - C*Xh);

    P = P - K*C*P;

    yout(i) = C*Xh;

end



plot(t, ys, t, yout);

, t, yout);

只看该作者 · 2011-12-8 21:50

使用最小二乘法例子的代码：

复制

 

 

Ts = 1;               采样时间

s = tf('s');

sysc = 100/(60*s +1);   真实系统的传递函数

sysd = c2d(sysc, Ts);

 

t = 0:Ts:(Ts*300);

u = ones(1, length(t));   系统输入

ys = lsim(sysd, u, t, 0); 系统输出

ys = ys + 10*rand(size(ys)); 测量量 = 系统输出 + 噪声

 

[numd, dend] = LeastSquare(u, ys, 3);   最小二乘法获取预测系统模型

[A, B, C, D] = tf2ss(numd, dend);       变换到状态空间形式

Len = length(u);

N = length(A);         系统维数 

 

yout = zeros(Len, 1);  滤波器输出

Xh = zeros(N, 1);      状态变量

P = eye(N);

Q = 0.02 * eye(N);     系统噪声

R = 50;                测量噪声

for i = 1 : Len

    Xh = A*Xh + B*u(i);

    P = A*P*A' + Q;

 

    K = P*C'* inv(C*P*C' + R);

    Xh = Xh + K*(ys(i) - C*Xh);

    P = P - K*C*P;

    yout(i) = C*Xh;

end

 

plot(t, ys, t, yout);