As shown in following code. inp contains 10*4 matrix. I am processing each row but i am getting processed output for the first row and for remaining row I am getting NaN value. why I am getting like that? where is the error?

% I am using this code in simulink model, where Input matrix(10*4) will change for every 1/60 seconds.

%code

function op = fnc(inp)

% define the filter

       op = zeros(10,4);

        F = [ 1 0 1 0 ;         % A
              0 1 0 1 ; 
              0 0 1 0 ; 
              0 0 0 1 ];

        H = [ 1 0 0 0;0 1 0 0;0 0 0 0;0 0 0 0 ] ; 

        R = [1 0 0 0 ; 0 1 0 0; 0 0 1 0 ; 0 0 1 0];

        X = zeros(1,4);
        Q = eye(1)*1e-5;
        w = 0.2;
        P = eye(4)*10;

for i = 1 : 10

        x = inp(i,:);
        for m = 1
            for n = 2
                if (x(m,n)==0)
                      op(i,:) = x;

                elseif (0<x(m,n))&&(x(m,n)<70)

                    X = X*F ;        %predicted state (1*4)

                    S = H'*H*P' + R;     %measurement error covariance (4*4)
                    K = (H*P')/S;  %optimal Kalman gain (4*4)
                    P = P - K*H*P; %(4*4)
                    P = F*F'*P + Q;     % estimate error covariance (4*4)
                    y = x - X*H;        %measurement error/residual (1*4)
                    X = X + y*K;      %updated state estimate (1*4)
                    X_1 = X*H;

                   op(i,:) = X_1;

                else
                     %Predictor equations
                    X = X*F + w;        %predicted state (4*1)
                    P = F*P*F' + Q;     % estimate error covariance (4*4)

                    op(i,:) = X;
                end

            end

        end

end

[Sachin Patil]

Please help I am facing this problem from the last 2 days.

[dbmn]

Are you sure that the two following lines are correct? (they don't Loop into anything)

for m = 1
   for n = 2

[Image Analyst]

No need for loops then. Simply get rid of the two for loops and use the 1,2 index

if (x(1,2)==0)

Answer 1

[dbmn]

When I evaluate your code I always get a warning on the following line: (it is between 0 and 70).

K = (H*P')/S;  %optimal Kalman gain (4*4)
Warning: Matrix is singular to working precision.

It seems that the divison at that point is no good idea because the rank of those Matrices is not full. If you change the diagonal Elements of the H Matrix to anything not Zero it runs withouth producing NANs.

[dbmn]

Or use the back division Operator with your values of H

K = (H*P')\S;

please cross check if this produces the desired results, as it is a different operation as /

[Sachin Patil]

I changed H matrix elements but I am getting results with difference of 6. i.e. if my input is 37.3 then my output is 31.2, actually I should get near to 37. Is there problem in h matrix assignment?

Answer 2

[Massimo Zanetti]

In these two lines anything is looping,

x = inp(i,:);
        for m = 1
            for n = 2

put something like this:

x = inp(i,:);
        for m = 1:SOMETHING
            for n = 2:SOMETHING ELSE

[Sachin Patil]

I used like that because I want to process based on 2nd element of each row.