扩展卡尔曼S函数编程问题

leaonliu

function [sys,x0,str,ts] = EKFLITER(t,x,u,flag)
%This S-Function is to indentify the inertia online using extended kalman
%filter method.
%x(1)=w(estimate speed)
%x(2)=TL(load torque)
%x(3)=1/J(J is inertia of motor)
%u(1)=Te
%u(2)=w(the real speed)
switch flag,
    case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;  
    case 2,
    sys=mdlUpdate(t,x,u);
    case 3,
    sys=mdlOutputs(t,x,u);
    case {1,4,9},
    sys=[];
otherwise
    error(['Unhandled flag = ',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 3;
sizes.NumOutputs     = 2;
sizes.NumInputs      = 2;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;   
sys = simsizes(sizes);
x0 = [0 0 1500];
str = [];
ts  =1e-4;   
function sys=mdlUpdate(t,x,u)
ts=1e-4;
A=[1 -x(3)*ts 0;0 1 0;0 0 1];
B=[x(3)*ts 0 0]';
P=diag([1 1 10]);
C=[1 0 0];
H=[1 0 0];
Q=diag([1e-5 1e-4 5e-6]);
R=5e-3;
y=u(2);
Y=C*x;
G=[1 -x(3)*ts u(1)-x(2)*ts;0 1 0;0 0 1];
%The first step
x=A*x+B*u(1);
P=G*P*G'+Q;%这里应该是用P=G*Pe*G’+Q但是由于Pe在之前没有定义所以不能用， 现在的问题在于，本来是一个迭代循环，可是现在P每次更新都被重置为diag[1 1 10],所以程序的收敛速度非常慢。看看哪位朋友能帮忙解决一下，就是起始的P值为diag[1 1 10]，之后P=G*Pe*G’+Q，多谢大家了，已经弄了好几天了，没有找到方法。
%The second step
K=P*H'/(H*P*H'+R);
Xe=x+K*[y-Y];
Pe=P-K*H*P;
%Result
sys(1)=Xe(1);
sys(2)=Xe(2);
sys(3)=Xe(3);
function sys=mdlOutputs(t,Xe,u)
%Output sys(1) is load torque
%sys(2) is inertia J
sys(1)=Xe(2);
sys(2)=1/Xe(3);

qibbxxt

1. doc persistent

复制代码

lin2009

P=diag([1 1 10]);放错位置了。

从你要表达的意思看，P=diag([1 1 10]);是在初始化阶段（flag=0）完成的工作，应该把它放在
function [sys,x0,str,ts]=mdlInitializeSizes子函数中。

anbcjys

可以用if判断

anbcjys

可以用if判断

leaonliu

TO：lin2009，初始化那个函数里是可以自己加参数的么
function [sys,x0,str,ts,P]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 3;
sizes.NumOutputs     = 2;
sizes.NumInputs      = 2;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;   
sys = simsizes(sizes);
x0 = [0 0 1500];
P=diag([1 1 10]);
str = [];
ts  =1e-4;   
如果可以的话是这样进行P初值的设置么
To：anbcjys
你说的if是什么意思呢，是对时间的if嘛？
if ((t>=0)&&(t<=0.01))
P=G*P*G'+Q;
else
P=G*Pe*G'+Q;
你是这个意思么？可是我这样编的时候会告知出错，也就是用前一段时间的Pe来带入现在的P是不允许的，不知道怎么弄

lin2009

1、可以采用函数参数传递的方式，避免每执行一次子函数就重新赋值一次（重复无用），如P,C,H,Q,R。

TO：lin2009，初始化那个函数里是可以自己加参数的么
function [sys,x0,str,ts,P]=mdlInitializeSizes
s-functon函数及子函数的输入、输出都有固定的格式。
输出不能改变，如初始化子函数的输出就是 [sys,x0,str,ts]，不能改变，不能自己加参数。
但是输入可以自己加参数。
因此，你的一些参数如P,C,H,Q,R等与仿真系统变量无关的常数项，可以考虑直接利用参数传递进来。如function [sys,x0,str,ts] = EKFLITER(t,x,u,flag)
可以改为function [sys,x0,str,ts] = EKFLITER(t,x,u,flag,P,C,H,Q,R)
2、若不通过上面用函数参数传递的方式。在各子函数之间共用一个变量（即公共变量），则需要声明是gobal类型或persistent类型。
参考程序：

1. function [ sys, x0, str, ts ] = EKFLITER(t, x, u, flag)
   
2. %This S - Function is to indentify the inertia online using extended kalman
   
3. %filter method.
   
4. %x(1) = w(estimate speed)
   
5. %x(2) = TL(load torque)
   
6. %x(3) = 1/J(J is inertia of motor)
   
7. %u(1) = Te
   
8. %u(2) = w(the real speed)
   
9. switch flag,
   
10.     case 0,
    
11.         [ sys, x0, str, ts ] = mdlInitializeSizes;
    
12.     case 2,
    
13.         sys = mdlUpdate(t, x, u);
    
14.     case 3,
    
15.         sys = mdlOutputs(t, x, u);
    
16.     case {1, 4, 9},
    
17.         sys = [ ];
    
18.     otherwise
    
19.         error([ 'Unhandled flag = ', num2str(flag) ]);
    
20. end
    
21. function [ sys, x0, str, ts ] = mdlInitializeSizes
    
22. sizes = simsizes;
    
23. sizes.NumContStates = 0;
    
24. sizes.NumDiscStates = 3;
    
25. sizes.NumOutputs = 2;
    
26. sizes.NumInputs = 2;
    
27. sizes.DirFeedthrough = 0;
    
28. sizes.NumSampleTimes = 1;
    
29. sys = simsizes(sizes);
    
30. x0 = [ 0 0 1500 ];
    
31. str = [ ];
    
32. ts = 1e-4;
    
33. function sys = mdlUpdate(t, x, u)
    
34. ts = 1e-4;
    
35. A = [ 1 - x(3)*ts 0; 0 1 0; 0 0 1 ];
    
36. B = [ x(3)*ts 0 0 ]';
    
37. % P = diag([ 1 1 10 ]);
    
38. C = [ 1 0 0 ];
    
39. H = [ 1 0 0 ];
    
40. Q = diag([ 1e-5 1e-4 5e-6 ]);
    
41. R = 5e-3;
    
42. y = u(2);
    
43. Y = C*x;
    
44. G = [ 1 - x(3)*ts u(1) - x(2)*ts; 0 1 0; 0 0 1 ];
    
45. %The first step
    
46. x = A*x + B*u(1);
    
47. 
    
48. persistent P
    
49. if isempty(P)
    
50.     P = diag([ 1 1 10 ]);
    
51. else
    
52.     P = G*P*G' + Q;
    
53. end
    
54. 
    
55. % 可以将C H Q R 等变量按上面的方式定义为persistent。
    
56. % 注意顺序
    
57. 
    
58. %
    
59. % P = G*P*G' + Q;%这里应该是用P = G*Pe*G’ + Q但是由于Pe在之前没有定义所以不能用, 现在的问题在于, 本来是一个迭代循环, 可是现在P每次更新都被重置为diag[ 1 1 10 ], 所以程序的收敛速度非常慢。看看哪位朋友能帮忙解决一下, 就是起始的P值为diag[ 1 1 10 ], 之后P = G*Pe*G’ + Q, 多谢大家了, 已经弄了好几天了, 没有找到方法。
    
60. % %The second step
    
61. 
    
62. K = P*H'/(H*P*H' + R);
    
63. Xe = x + K*( y - Y );
    
64. Pe = P - K*H*P;
    
65. %Result
    
66. sys(1) = Xe(1);
    
67. sys(2) = Xe(2);
    
68. sys(3) = Xe(3);
    
69. function sys = mdlOutputs(t, Xe, u)
    
70. %Output sys(1) is load torque
    
71. %sys(2) is inertia J
    
72. sys(1) = Xe(2);
    
73. sys(2) = 1/Xe(3);

复制代码

prettyboylin520

学习了，很受用，谢谢楼主！