Prev: how to find the center of the circle passing trrough two points no radiustwo
Next: Matrix Indexing
From: Mohammad Salehizadeh on 26 Mar 2010 10:23
I want to implement Kalman filtering as a part of my main algoritm.I classified Kalman parameters as below:

w(n): Kalman cefficient
autocorrelation of state noise : Qvv(n)
autocorrelation of measurement noise Qnn(n)

Assume that I have below signal information as below:
u(n): Input filter signal=Desired signal+noise;
d(n): Desired signal
alpha(n)=d(n)-u(n)'*w(n)

I don't know how state noise and mesurement noise in Kalman algorithm is calculated according by Desired signal d or u input filter signal till I awill be able to apply them in my program.

Thanks alot.

Regards,
Mohammad
From: AJ on 26 Mar 2010 15:00
"Mohammad Salehizadeh" <en66salehi.mohammad(a)example.com> wrote in message <hoig09$d3j$1(a)fred.mathworks.com>...
> I want to implement Kalman filtering as a part of my main algoritm.I classified Kalman parameters as below:
>
> w(n): Kalman cefficient
> autocorrelation of state noise : Qvv(n)
> autocorrelation of measurement noise Qnn(n)
>
> Assume that I have below signal information as below:
> u(n): Input filter signal=Desired signal+noise;
> d(n): Desired signal
> alpha(n)=d(n)-u(n)'*w(n)
>
> I don't know how state noise and mesurement noise in Kalman algorithm is calculated according by Desired signal d or u input filter signal till I awill be able to apply them in my program.
>
> Thanks alot.
>
> Regards,
> Mohammad


It's been a while since I used this, but maybe this will give you some insight. See Gelb's book for more info. Good luck!

function [x,P,xtemp] = kalman(x, P, z, dt, H, A, V, Q)
% kalman - implementation of a generic Kalman filter.
%
% [x,P,xtemp] = kalman(x, P, z, dt, H, A, V, Q)
% Inputs:
% x - Previous State estimate vector, x(k-1,+), of size n x 1
% P - Previous convarience matrix, P(k-1,+) of size n x n
% z - Measurement vector, z(k), of size l x 1
% dt - time interval since last update (scaler)
% H - Observation matrix, of size l x n
% A - dynamics matrix, of size n x n
% V - Measurement noise matrix, of size l x l
% Q - Process noise matrix (optional), of size n x n
% Outputs:
% x - New State esimate vector, x(k,+), of size n x 1
% P - New convarience matrix, P(k,+) of size n x n

%---------------------------------------------------------------
% ALOGRITHM DESCRIPTION:
%
% State and Covarience Propogate
% PHI(k-1) = I + A(k-1)*dt
% dX(k,-) = PHI(k-1)*dX(k-1,+)
% P(k,-) = PHI(k-1)*P(k-1,+)*PHI(k-1)' + Q(k-1)*dt
% P(k,-) = 0.5*[P(k,-) + P'(k,-)] % Numerical shaper to ensure symmetry
%
% State Update
% -1
% K(k) = P(k,-)*H'[H*P(k,-)*H' + V]
% P = (I - K(k)*H)*P
% X(k,-) = Xins(k,-) + dX(k,-)
% dX(k,+) = dX(k,-) + K(k)*[z(k) - HX(k,-)]
%
%---------------------------------------------------------------
% State and covariance Propogate
if ~exist('Q') Q = zeros(size(P)); end %

I = diag(ones(1,length(A))); % Identity Matrix
PHI = I + A*dt; % temporary variable
xtemp = (A*dt)*x;
x = PHI * x; % Update state estimate per dynamics matrix
P = PHI * P * PHI' + Q*abs(dt); % Update covariance matrix, w/process noise
P = 0.5 * (P + P'); % Shape covarience matrix to ensure symmetry

%---------------------------------------------------------------
% State Update

K = P * H' * inv(H * P * H' + V); % Kalman gain matrix
P = (I - K * H) * P; % Update covarience matrix
x = x + K * (z - H * x); % New state estimation

return % All done.
 | 
Pages: 1
Prev: how to find the center of the circle passing trrough two points no radiustwo
Next: Matrix Indexing