1 | /************************************ |
---|
2 | Extended Kalman Filter |
---|
3 | Matrix operations |
---|
4 | |
---|
5 | V. Smidl, Z. Peroutka |
---|
6 | |
---|
7 | Rev. 28.10.2010 (ZP) |
---|
8 | |
---|
9 | 26.10.2010 Prvni verze (VS) |
---|
10 | |
---|
11 | 26.10.2010 Upravena chyba v Thorton_fast - spatne shiftovani o vypoctu SIGMA. |
---|
12 | 27.10.2010 Pokus o odstraneni problemu v Thorton_fast - potize dela omezovani (orezavani) varianci. |
---|
13 | 28.10.2010 Drobne upravy v kodu. |
---|
14 | |
---|
15 | *************************************/ |
---|
16 | |
---|
17 | #include "matrix_vs.h" |
---|
18 | #include "qmath.h" |
---|
19 | |
---|
20 | #define HIGH_PRECISION 1 |
---|
21 | |
---|
22 | void householder(int16 *Ch, int16 *Q, unsigned int16 dimx) |
---|
23 | { |
---|
24 | int16 k,j,i; |
---|
25 | int16 alpha,beta; |
---|
26 | int32 sigma; // 2*qCh |
---|
27 | int32 tmp_long; |
---|
28 | int16 B[25];//Q in qCh |
---|
29 | int16 w[5]; |
---|
30 | int16 v[5]; |
---|
31 | |
---|
32 | int16 *B_ij, *Q_i, *B_kj, *Ch_kj, *Ch_ij, *w_j, *v_j; |
---|
33 | |
---|
34 | B_ij=B; |
---|
35 | Q_i=Q; |
---|
36 | // copy Q to B |
---|
37 | for (i=0;i<dimx*dimx;i++) |
---|
38 | { |
---|
39 | *B_ij++=(*Q_i++)>>(15-qCh); |
---|
40 | } |
---|
41 | |
---|
42 | for (k=dimx-1; k>=0; k--) |
---|
43 | { |
---|
44 | sigma=0; |
---|
45 | B_kj=B+k*dimx+k; |
---|
46 | Ch_kj=Ch+k*dimx; |
---|
47 | |
---|
48 | for (j=k;j<dimx ;j++) |
---|
49 | { |
---|
50 | sigma+=((long)*B_kj**B_kj); |
---|
51 | B_kj++; |
---|
52 | } |
---|
53 | |
---|
54 | for (j=0;j<=k;j++) |
---|
55 | { |
---|
56 | sigma+=((long)*Ch_kj**Ch_kj); |
---|
57 | Ch_kj++; |
---|
58 | } |
---|
59 | |
---|
60 | //alpha in qCh |
---|
61 | // alpha = (int)(sqrt((double)sigma)+0.5); // verze pro PC |
---|
62 | alpha = qsqrt(sigma); // verze pro DSP |
---|
63 | |
---|
64 | sigma=0; |
---|
65 | |
---|
66 | w_j=w; |
---|
67 | B_kj=B+k*dimx; |
---|
68 | |
---|
69 | for (j=0;j<dimx;j++) |
---|
70 | { |
---|
71 | *w_j=*B_kj; |
---|
72 | sigma+=(long)*w_j**w_j; |
---|
73 | w_j++; |
---|
74 | B_kj++; |
---|
75 | } |
---|
76 | |
---|
77 | v_j=v; |
---|
78 | Ch_kj=Ch+k*dimx; |
---|
79 | |
---|
80 | for (j=0; j<=k;j++) |
---|
81 | { |
---|
82 | if (j==k) { |
---|
83 | *v_j=*Ch_kj-alpha; |
---|
84 | } else { |
---|
85 | *v_j=*Ch_kj; |
---|
86 | } |
---|
87 | sigma+=(long)*v_j**v_j; |
---|
88 | |
---|
89 | v_j++; |
---|
90 | Ch_kj++; |
---|
91 | } |
---|
92 | |
---|
93 | alpha=sigma>>(qCh+1); // alpha = sigma /2; |
---|
94 | if (alpha==0) alpha =1; |
---|
95 | |
---|
96 | for (i=0;i<=k;i++) |
---|
97 | { |
---|
98 | sigma=0; |
---|
99 | |
---|
100 | B_ij=B+i*dimx+i; |
---|
101 | w_j=w+i; |
---|
102 | |
---|
103 | for (j=i;j<dimx;j++) |
---|
104 | { |
---|
105 | sigma+=((long)*B_ij**w_j); |
---|
106 | |
---|
107 | B_ij++; |
---|
108 | w_j++; |
---|
109 | } |
---|
110 | |
---|
111 | Ch_ij = Ch + i*dimx; |
---|
112 | v_j=v; |
---|
113 | |
---|
114 | for (j=0;j<=k;j++) |
---|
115 | { |
---|
116 | sigma+=(long)*Ch_ij**v_j; |
---|
117 | |
---|
118 | Ch_ij++; |
---|
119 | v_j++; |
---|
120 | } |
---|
121 | |
---|
122 | sigma = sigma >> 15; // navrat do Q15 |
---|
123 | if (sigma>32767) sigma=32767; |
---|
124 | |
---|
125 | |
---|
126 | B_ij=B+i*dimx+i; |
---|
127 | w_j=w+i; |
---|
128 | |
---|
129 | for (j=i;j<dimx;j++) |
---|
130 | { |
---|
131 | tmp_long=((long)*B_ij*alpha-sigma**w_j)/alpha; |
---|
132 | if (tmp_long>32767) |
---|
133 | tmp_long=32767; |
---|
134 | if (tmp_long<-32768) |
---|
135 | tmp_long=-32768; |
---|
136 | *B_ij++=tmp_long; |
---|
137 | w_j++; |
---|
138 | }; |
---|
139 | |
---|
140 | Ch_ij=Ch+i*dimx; |
---|
141 | v_j=v; |
---|
142 | |
---|
143 | for (j=0;j<=k;j++) |
---|
144 | { |
---|
145 | tmp_long=((long)*Ch_ij*alpha-sigma**v_j)/alpha; |
---|
146 | if (tmp_long>32767) |
---|
147 | tmp_long=32767; |
---|
148 | if (tmp_long<-32768) |
---|
149 | tmp_long=-32768; |
---|
150 | *Ch_ij++=tmp_long; |
---|
151 | v_j++; |
---|
152 | } |
---|
153 | } |
---|
154 | } |
---|
155 | } |
---|
156 | |
---|
157 | void carlson(int16 *difz, int16 *xp, int16 *Ch, int16 *R, unsigned int16 dimy, unsigned int16 dimx ) { |
---|
158 | int16 alpha,beta,gamma; |
---|
159 | int16 delta, eta,epsilon,zeta,sigma,tau; |
---|
160 | int16 i,j,iy; |
---|
161 | int16 w[5]; |
---|
162 | int32 tmp_long; |
---|
163 | |
---|
164 | int16 *Ch_ij, *w_i, *x_i, *Ch_iyj; |
---|
165 | |
---|
166 | |
---|
167 | for (iy=0; iy<dimy; iy++) |
---|
168 | { |
---|
169 | alpha=R[iy]; |
---|
170 | delta = difz[iy]; |
---|
171 | |
---|
172 | Ch_iyj=Ch+iy*dimx; |
---|
173 | |
---|
174 | for (j=0;j<dimx;j++) |
---|
175 | { |
---|
176 | sigma=*Ch_iyj++; |
---|
177 | beta=alpha; |
---|
178 | // alpha+=((long)sigma*sigma)>>15; |
---|
179 | alpha=(((long)alpha<<15)+(long)sigma*sigma)>>15; // vyssi presnost |
---|
180 | gamma= qsqrt(((long)alpha*beta)); // predelat v DSP |
---|
181 | w[j]=0; |
---|
182 | |
---|
183 | Ch_ij=Ch+j; |
---|
184 | w_i=w; |
---|
185 | |
---|
186 | for (i=0;i<=j;i++) |
---|
187 | { |
---|
188 | tau=*Ch_ij; |
---|
189 | tmp_long=((long)beta**Ch_ij -(long)sigma**w_i)/gamma; |
---|
190 | |
---|
191 | /* *Ch_ij=tmp_long; |
---|
192 | if (tmp_long>32767) |
---|
193 | *Ch_ij=32767; |
---|
194 | if (tmp_long<-32768) |
---|
195 | *Ch_ij=-32768; /**/ |
---|
196 | if (tmp_long>32767) |
---|
197 | tmp_long=32767; |
---|
198 | if (tmp_long<-32768) |
---|
199 | tmp_long=-32768; |
---|
200 | *Ch_ij=tmp_long; |
---|
201 | |
---|
202 | // w_i+=((long)tau*sigma)>>15; |
---|
203 | *w_i=(((long)*w_i<<15)+(long)tau*sigma)>>15; |
---|
204 | |
---|
205 | w_i++; |
---|
206 | Ch_ij+=dimx; |
---|
207 | } |
---|
208 | } |
---|
209 | |
---|
210 | x_i=xp; |
---|
211 | w_i=w; |
---|
212 | for (i=0;i<dimx;i++) { |
---|
213 | // xp[i]+=((long)w[i]*delta)/alpha; |
---|
214 | // *x_i+=((long)*w_i*delta)/alpha; |
---|
215 | *x_i=((long)*x_i*alpha+(long)*w_i*delta)/alpha; // vyssi presnost |
---|
216 | x_i++; |
---|
217 | w_i++; |
---|
218 | } |
---|
219 | } |
---|
220 | } |
---|
221 | |
---|
222 | /* Matrix multiply Full matrix by upper diagonal matrix; */ |
---|
223 | void mmultACh(int16 *m1, int16 *up, int16 *result, unsigned int16 rows, unsigned int16 columns) { |
---|
224 | unsigned int16 i, j, k; |
---|
225 | int32 tmp_sum=0L; |
---|
226 | int16 *m2pom; |
---|
227 | int16 *m1pom=m1; |
---|
228 | int16 *respom=result; |
---|
229 | |
---|
230 | for (i=0; i<rows; i++) //rows of result |
---|
231 | { |
---|
232 | for (j=0; j<columns; j++) //columns of result |
---|
233 | { |
---|
234 | m2pom=up+j; |
---|
235 | |
---|
236 | for (k=0; k<=j; k++) //inner loop up to "j" - U(j,j)==1; |
---|
237 | { |
---|
238 | tmp_sum+=(int32)(*(m1pom++))**m2pom; |
---|
239 | m2pom+=columns; |
---|
240 | } |
---|
241 | m1pom-=(j+1); // shift back to first element |
---|
242 | |
---|
243 | *respom++=tmp_sum>>15; |
---|
244 | |
---|
245 | tmp_sum=0; |
---|
246 | } |
---|
247 | m1pom+=(columns); |
---|
248 | } |
---|
249 | } |
---|
250 | |
---|
251 | void givens(int16 *Ch, int16 *Q, unsigned int16 dimx) |
---|
252 | { |
---|
253 | int16 i,j,k; |
---|
254 | int16 rho,s,c,tau; |
---|
255 | int32 tmp_long; |
---|
256 | |
---|
257 | int16 A[25];//beware |
---|
258 | |
---|
259 | int16 *A_ij, *Q_i, *Ch_ki, *Ch_kj, *Ch_ii, *Ch_ij, *A_kj; |
---|
260 | |
---|
261 | A_ij=A; |
---|
262 | Q_i=Q; |
---|
263 | // copy Q to A |
---|
264 | for (i=0;i<dimx*dimx;i++) |
---|
265 | { |
---|
266 | *A_ij++=(*Q_i++)>>(15-qCh); |
---|
267 | } |
---|
268 | |
---|
269 | for (i=dimx-1; i>=0; i--) |
---|
270 | { |
---|
271 | Ch_ii=Ch+i*dimx+i; |
---|
272 | A_ij=A+i*dimx; |
---|
273 | |
---|
274 | for (j=0; j<dimx; j++) |
---|
275 | { |
---|
276 | if (*A_ij!=0) |
---|
277 | { |
---|
278 | tmp_long=(long)*Ch_ii**Ch_ii+(long)*A_ij**A_ij; |
---|
279 | |
---|
280 | rho=qsqrt(tmp_long); |
---|
281 | s=((long)*A_ij<<15)/rho; |
---|
282 | c=((long)*Ch_ii<<15)/rho; |
---|
283 | |
---|
284 | Ch_ki=Ch+i; |
---|
285 | A_kj=A+j; |
---|
286 | |
---|
287 | for (k=0;k<=i; k++) |
---|
288 | { |
---|
289 | tau=((long)c**A_kj-(long)s**Ch_ki)>>15; |
---|
290 | *Ch_ki=((long)s**A_kj+(long)c**Ch_ki)>>15; |
---|
291 | *A_kj=tau; |
---|
292 | |
---|
293 | Ch_ki+=dimx; |
---|
294 | A_kj+=dimx; |
---|
295 | } |
---|
296 | |
---|
297 | } |
---|
298 | |
---|
299 | A_ij++; |
---|
300 | } |
---|
301 | |
---|
302 | Ch_ij = Ch+i*dimx; |
---|
303 | |
---|
304 | for (j=0; j<i; j++) |
---|
305 | { |
---|
306 | if (*Ch_ij!=0) |
---|
307 | { |
---|
308 | tmp_long=(long)*Ch_ii**Ch_ii+(long)*Ch_ij**Ch_ij; |
---|
309 | rho=qsqrt(tmp_long); |
---|
310 | s=((long)*Ch_ij<<15)/rho; |
---|
311 | c=((long)*Ch_ii<<15)/rho; |
---|
312 | |
---|
313 | Ch_kj = Ch + j; |
---|
314 | Ch_ki = Ch + i; |
---|
315 | |
---|
316 | for (k=0; k<=i; k++) |
---|
317 | { |
---|
318 | tau=((long)c**Ch_kj-(long)s**Ch_ki)>>15; |
---|
319 | *Ch_ki =((long)s**Ch_kj+(long)c**Ch_ki)>>15; |
---|
320 | *Ch_kj=tau; |
---|
321 | |
---|
322 | Ch_kj += dimx; |
---|
323 | Ch_ki += dimx; |
---|
324 | } |
---|
325 | } |
---|
326 | |
---|
327 | Ch_ij++; |
---|
328 | } |
---|
329 | } |
---|
330 | } |
---|