Index: applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp
===================================================================
--- applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1321)
+++ applications/pmsm/simulator_zdenek/ekf_example/matrix_vs.cpp (revision 1326)
@@ -206,5 +206,5 @@
 				*U_ij -=  ((int32)(*a_j)*(*b_i))/lambda;         // pozadovane optimalni reseni
 				 *b_i  +=  ((int32)beta*(*b_j))>>15;
-				 printf("%d, %d, %d\n", ((int32)(*a_j)*(*b_i))/lambda, *b_i, beta);
+				 //printf("%d, %d, %d\n", ((int32)(*a_j)*(*b_i))/lambda, *b_i, beta);
 			}
 		}
@@ -451,5 +451,5 @@
                 tau=Ch[i*dimx+j];
 				tmp_long=((int32)beta*Ch[i*dimx+j] -(int32)sigma*w[i])/gamma;
-				if (tmp_long>32767) {
+		/*		if (tmp_long>32767) {
 					Ch[i*dimx+j]=32767;
 				} else {
@@ -457,8 +457,8 @@
 						Ch[i*dimx+j]=-32767;
 					} else {
-						Ch[i*dimx+j]=tmp_long;
-					}
+		*/				Ch[i*dimx+j]=tmp_long;
+			/*		}
 				}
-				
+			*/	
                 w[i]+=((int32)tau*sigma)>>15;
             }
@@ -537,7 +537,14 @@
 			}
 			m1pom-=(j+1); // shift back to first element
-			
-			*respom++=tmp_sum>>15;
-			
+	
+		/*	if (tmp_sum>(1<<29)-1) 
+				*respom++=(1<<14);
+			else
+		*/	
+		if (tmp_sum>0)
+			*respom++=(tmp_sum+(1<<14)-1)>>15;
+		else 
+			*respom++=(tmp_sum-(1<<14)+1)>>15;
+		
 			tmp_sum=0;
 		}
@@ -552,4 +559,6 @@
 	int16 rho,s,c,tau;
 	int32  tmp_long;
+	
+	//c,s in q14!!
 	
 	int16 A[25];//beware
@@ -573,14 +582,11 @@
 		for (j=0; j<dimx; j++) 
 		{
-			//			tmp_long=(long)Ch[i*dimx+i]*Ch[i*dimx+i]+(long)A[i*dimx+j]*A[i*dimx+j];
-			
-			tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*A_ij**A_ij;
-			
-			if (tmp_long>0)
+			if (*A_ij!=0)
 			{
+				tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*A_ij**A_ij;
 				//				rho=qsqrt(tmp_long);                   // verze pro DSP
 				rho=(int16)(sqrt((double)tmp_long));     // verze pro PC
-				s=((int32)*A_ij<<15)/rho;
-				c=((int32)*Ch_ii<<15)/rho;
+				s=(((int32)*A_ij)<<14)/rho;
+				c=(((int32)*Ch_ii)<<14)/rho;
 				
 				Ch_ki=Ch+i;
@@ -589,6 +595,10 @@
 				for (k=0;k<=i; k++)
 				{
-					tau=((int32)c**A_kj-(int32)s**Ch_ki)>>15;
-					*Ch_ki=((int32)s**A_kj+(int32)c**Ch_ki)>>15;
+					tau=((int32)c**A_kj-(int32)s**Ch_ki)>>14;
+					tmp_long=(int32)s**A_kj+(int32)c**Ch_ki;
+					if (tmp_long>(1<<28)) //q14 + q15
+						*Ch_ki = (1<<14)-1;
+					else 
+						*Ch_ki=tmp_long>>14;
 					*A_kj=tau;
 					
@@ -604,12 +614,16 @@
 		for (j=0; j<i; j++)
 		{
-			tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*Ch_ij**Ch_ij;
-			
-			if (tmp_long>0)
+			
+			if (*Ch_ij>0)
 			{
+				tmp_long=(int32)*Ch_ii**Ch_ii+(int32)*Ch_ij**Ch_ij;
 				//			rho=qsqrt(tmp_long);                     // verze pro DSP
-				rho=(int16)(sqrt((double)tmp_long));       // verze pro PC
-				s=((int32)*Ch_ij<<15)/rho;
-				c=((int32)*Ch_ii<<15)/rho;
+				if (tmp_long>(1<<30)-1)
+					rho=(1<<15)-1;
+				else
+					rho=(int16)(sqrt((double)tmp_long));       // verze pro PC
+						
+				s=(((int32)*Ch_ij)<<14)/rho;
+				c=(((int32)*Ch_ii)<<14)/rho;
 				
 				Ch_kj = Ch + j;
@@ -618,6 +632,10 @@
 				for (k=0; k<=i; k++)
 				{
-					tau=((int32)c**Ch_kj-(int32)s**Ch_ki)>>15;
-					*Ch_ki =((int32)s**Ch_kj+(int32)c**Ch_ki)>>15;
+					tau=((int32)c**Ch_kj-(int32)s**Ch_ki)>>14;
+					tmp_long =((int32)s**Ch_kj+(int32)c**Ch_ki);
+					if (tmp_long>(1<<28))
+						*Ch_ki = (1<<14)-1;
+					else 
+						*Ch_ki=tmp_long>>14;
 					*Ch_kj=tau;
 					
@@ -720,14 +738,19 @@
 			
 			//sigma=Ch[iy*dimx+j];
-			beta=alpha;
+			beta=alpha; // in q15
 			//            alpha+=((long)sigma*sigma)>>15;
-			alpha=(((int32)alpha<<15)+(int32)sigma*sigma)>>15;				// vyssi presnost
+			tmp_long=((int32)alpha<<15)+(((int32)sigma*sigma)<<(30-2*qCh));
+			if (tmp_long>0)
+				alpha=(tmp_long+(1<<14)-1)>>15;				// vyssi presnost
+			else
+				alpha=(tmp_long-(1<<14)+1)>>15;				// vyssi presnost
+					
 			//            gamma= qsqrt(((long)alpha*beta));                          // verze pro DSP
 			gamma= (int16)(sqrt((double)((int32)alpha*beta)));              // verze pro PC
-			
+			       // in q15
 			w[j]=0;
 			
 			Ch_ij=Ch+j;
-			w_i=w;
+			w_i=w; // in q15
 			
 			for (i=0;i<=j;i++)
@@ -736,14 +759,19 @@
 				tau=*Ch_ij;
 				//				tmp_long=((long)beta*Ch[i*dimx+j] -(long)sigma*w[i])/gamma;
-				tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma;
-				
-				if (tmp_long>32767)
+				tmp_long=((int32)beta**Ch_ij -(int32)sigma**w_i)/gamma; // in qCh
+				
+/*				if (tmp_long>32767)
 					tmp_long=32767;
 				if (tmp_long<-32768)
-					tmp_long=-32768;
+					tmp_long=-32768;*/
 				*Ch_ij=tmp_long;
 				
 				//                w_i+=((long)tau*sigma)>>15;
-				*w_i=(((int32)*w_i<<15)+(int32)tau*sigma)>>15;
+				tmp_long = ((int32)*w_i<<15)+((int32)tau*sigma<<(30-2*qCh));
+				/*if (tmp_long>0)
+					*w_i=(tmp_long+(1<<14)-1)>>15;
+				else 
+					*w_i=(tmp_long-(1<<14)+1)>>15;
+				*/ *w_i=(tmp_long)>>15;
 				
 				w_i++;
@@ -761,5 +789,6 @@
 			w_i++;
 		}
-	}
-}
-
+		printf("Kg: %d %d\n",w[0],w[1]);
+	}
+}
+
