Group # 7: Kalman Filter Based Parameter Estimation
Group # 7: Kalman Filter Based Parameter Estimation
Group # 7
Submitted to: Dr AQ Khan
Submitted by: Kamran Iftikhar-51997 Moazzam Nazir-50481 Muhammad Ahmed Muhammad Aqeel Abassi Muhammad Awais Tayyab-52251
2/27/2012
Lab 7
Kalman filter based parameter estimation/optimization
Kalman Filter:
The Kalman filter is a set of mathematical equations that provides an efficient computational (recursive) means to estimate the state of a process, in a way that minimizes the mean of the squared error. The filter is very powerful in several aspects: it supports estimations of past, present, and even future states, and it can do so even when the precise nature of the modeled system is unknown. Kalman filter is used in industrial navigation, radar tracking, signal processing, sensor calibration etc
Objective:
The objective of this lab was to write m-file , s-function and built a simulink model for parameter estimation using extended Kalman filter.
Algorithm:
Let some process
The Kalman filter estimates a process by using a form of feedback control: the filter estimates the process state at some time and then obtains feedback in the form of (noisy) measurements. As such, the equations for the Kalman filter fall into two groups: time update equations and measurement update equations. The time update equations can also be thought of as predictor equations, while the measurement update equations can be thought of as corrector equations. Predictor-corrector algorithm can be viewed as shown in figure
Task:
. To do this we take
This means First we discritize the plant by using T=0.01 second. We also take w=10 and zeta=0.7, so the discritze plant is
To estimate the value of , time update equations and measurement update equations are used recursively. For Matlab implementation code is written for these equations and for sfunctions same code is used in the s-function template file to generate a s-function for the given problem.
Matlab code:
T=.01; w=10; zeta=.7; A=[1 T 0;-0.1 1 0;0 0 1]; B=[0;12*T;0]; E=[0;T;0]; C=[1 0 0]; D=0; F=1;
xo=[0;0;0]; u=1; Po=[1 0 0;0 1 0;.1 0 1]; Q=[0 0 0;0 4.47 0;0 0 0]; R=.01; Pn=Po; Ew=[0;T;0]; Kk=[0;0;0]; %//////////////////////// x110=0;x220=0; x11=x110;x22=x220; x10=0;x20=0;x30=0; x1=x10;x2=x20;x3=x30; yh=0; xo0=[0;0;0]; Pn=Po; A=[1 T 0;-T*w*w 1-(2*T*w*x3) -2*T*w*x2;0 0 1]; y=[]; yhat=[]; origx1=[];origx2=[]; estx1=[];estx2=[];estx3=[]; for i=1:500 x11d=x11+T*x22; x22d=x22-T*w*w*x11-2*T*w*x22*zeta+12*T*u+T*(randn*(4.47)); y=[y;x11d+(randn*(.01))]; x11=x11d;x22=x22d; %//////// origx1=[origx1;x11d]; origx2=[origx2;x22d]; x1d=x1+T*x2; x2d=x2-T*w*w*x1-2*T*w*x2*x3-2*T*w*x3*x2+12*T*u; x3d=x3; A=[1 T 0;-T*w*w 1-(2*T*w*x3) -2*T*w*x2;0 0 1]; x1d=x1d+Kk(1)*(y(i)-x1d); x2d=x2d+Kk(2)*(y(i)-x1d); x3d=x3d+Kk(3)*(y(i)-x1d); Pn=A*Pn*A'+Ew*4.47*Ew'; Kk=Pn*C'*((C*Pn*C'+R)^-1); Pp=((eye(3)-Kk*C)*Pn);
Pn=Pp; yhat=[yhat;x1d]; x1=x1d;x2=x2d;x3=x3d; estx1=[estx1;x1]; estx2=[estx2;x2]; estx3=[estx3;x3]; end Kk x3 subplot(311) plot(origx1,'r') hold on plot(estx1) subplot(312) plot(origx2,'r') hold on plot(estx2) subplot(313) plot(estx3)
Results:
Implementation in Simulink:
Overall:
Plant:
Kalman Filter:
Results:
Plant State:
S-Function:
function msfuntmpl_basic(block) setup(block); function setup(block) % Register number of ports block.NumInputPorts = 4; block.NumOutputPorts = 1; % Setup port properties to be inherited or dynamic block.SetPreCompInpPortInfoToDynamic; block.SetPreCompOutPortInfoToDynamic; % Override input port properties %u(k-1) block.InputPort(1).Dimensions block.InputPort(1).DatatypeID = 0;
= 1; % double
block.InputPort(1).Complexity = 'Real'; block.InputPort(1).DirectFeedthrough = true; %z block.InputPort(2).Dimensions = 1; block.InputPort(2).DatatypeID = 0; % double block.InputPort(2).Complexity = 'Real'; block.InputPort(2).DirectFeedthrough = true; %Q(k-1) block.InputPort(3).Dimensions = [2,2]; block.InputPort(3).DatatypeID = 0; % double block.InputPort(3).Complexity = 'Real'; block.InputPort(3).DirectFeedthrough = true; %R(k) block.InputPort(4).Dimensions = 1; block.InputPort(4).DatatypeID = 0; % double block.InputPort(4).Complexity = 'Real'; block.InputPort(4).DirectFeedthrough = true; % Override output port properties block.OutputPort(1).Dimensions = 1; block.OutputPort(1).DatatypeID = 0; % double block.OutputPort(1).Complexity = 'Real'; % Register parameters block.NumDialogPrms % Register sample times block.SampleTimes = [0 0]; block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup); block.RegBlockMethod('InitializeConditions', @InitializeConditions); block.RegBlockMethod('Start', @Start); block.RegBlockMethod('Outputs', @Outputs); % Required block.RegBlockMethod('Update', @Update); block.RegBlockMethod('Derivatives', @Derivatives); block.RegBlockMethod('Terminate', @Terminate); % Required function DoPostPropSetup(block) block.NumDworks = 1; block.Dwork(1).Name block.Dwork(1).Dimensions block.Dwork(1).DatatypeID block.Dwork(1).Complexity block.Dwork(1).UsedAsDiscState = 0;
= = = = =
10
function Outputs(block) block.OutputPort(1).Data = block.Dwork(1).Data + block.InputPort(1).Data; function Update(block) block.Dwork(1).Data = block.InputPort(1).Data; function Derivatives(block) function Terminate(block)
11