1 /** @file range_kalman.c
2 *
3 * @brief This file contains Kalman filter for WLS range measurements
4 *
5 * Copyright 2023-2024 NXP
6 *
7 * SPDX-License-Identifier: BSD-3-Clause
8 *
9 */
10
11 #include <osa.h>
12 #if CONFIG_WLS_CSI_PROC
13 #include "range_kalman.h"
14 #include <stdio.h>
15 #include <math.h>
16
17 #ifdef RANGE_RUN_FLT
range_kalman(range_kalman_state * in)18 int range_kalman(range_kalman_state *in)
19 {
20 float range_hat, range_rate_hat;
21 float nu, S1;
22 float R1_11, R1_12, R1_22, W1_1, W1_2;
23 float delta_T = (float)(in->time - in->last_time) / 1000; // now seconds
24 float delta_T2 = delta_T * delta_T;
25 float delta_T3 = delta_T2 * delta_T;
26
27 // state propagation
28 // A = | 1 T |
29 // | 0 1 |
30 range_hat = in->last_range + delta_T * in->last_range_rate;
31 range_rate_hat = in->last_range_rate;
32 // residual
33 nu = in->range_measurement - range_hat; // H = [1; 0]
34
35 // propagated cov = A*R11*A' + Q
36 R1_11 = in->R0_11 + delta_T * 2 * in->R0_12 + delta_T2 * in->R0_22;
37 R1_12 = in->R0_12 + delta_T * in->R0_22;
38 R1_22 = in->R0_22;
39 // Q = | T^3/3 T^2/2 | * drive_var
40 // | T^2/2 T |
41 R1_11 += in->drive_var * delta_T3 / 3;
42 R1_12 += in->drive_var * delta_T2 / 2;
43 R1_22 += in->drive_var * delta_T;
44
45 // inovation cov S1 = y_err + H*R1*H'
46 S1 = in->measurement_var + R1_11;
47 // filter gain W1 = (R1*H')/S1
48 W1_1 = R1_11 / S1;
49 W1_2 = R1_12 / S1;
50
51 // updated covariance R11 = R1 - W1*S1*W1'
52 R1_11 -= W1_1 * S1 * W1_1;
53 in->R0_11 = (R1_11 > 0) ? R1_11 : 0;
54 R1_12 -= W1_1 * S1 * W1_2;
55 in->R0_12 = R1_12;
56 R1_22 -= W1_2 * S1 * W1_2;
57 in->R0_22 = (R1_22 > 0) ? R1_22 : 0;
58 // updated state
59 range_hat += W1_1 * nu;
60 in->last_range = (range_hat > 0) ? range_hat : 0;
61 range_rate_hat += W1_2 * nu;
62 in->last_range_rate = range_rate_hat;
63 in->last_time = in->time;
64
65 PRINTF("Kalman update R mat: %f m, %f m/s; d-time: %d ms; range rate %f m/s\r\n", (double)sqrtf(in->R0_11),
66 (double)sqrtf(in->R0_22), (int)(delta_T * 1000), (double)in->last_range_rate);
67 return 0; // no errors
68 }
69
range_kalman_init(range_kalman_state * in,float range,unsigned long long time,float range_drive_var,float range_measurement_var,float range_rate_init)70 void range_kalman_init(range_kalman_state *in,
71 float range,
72 unsigned long long time,
73 float range_drive_var,
74 float range_measurement_var,
75 float range_rate_init)
76 {
77 // initialize state
78 in->last_range = range;
79 in->last_range_rate = 0;
80 in->last_time = time;
81
82 in->R0_11 = range_measurement_var;
83 in->R0_12 = 0;
84 in->R0_22 = range_rate_init;
85
86 // initialize parameters
87 in->drive_var = range_drive_var;
88 in->measurement_var = range_measurement_var;
89 }
90
91 #else
range_kalman(range_kalman_state * in)92 int range_kalman(range_kalman_state *in)
93 {
94 int range_hat, range_rate_hat;
95 int nu, S1;
96 int R1_11, R1_12, R1_22, W1_1, W1_2;
97 unsigned int delta_T = in->time - in->last_time; // format ??
98 unsigned int delta_T2 = delta_T * delta_T;
99 unsigned int delta_T3 = delta_T2 * delta_T;
100
101 // state propagation
102 // A = | 1 T |
103 // | 0 1 |
104 range_hat = in->last_range + delta_T * in->last_range_rate;
105 range_rate_hat = in->last_range_rate;
106 // residual
107 nu = in->range_measurement - range_hat; // H = [1; 0]
108
109 // propagated cov = A*R11*A' + Q
110 R1_11 = in->R0_11 + delta_T * 2 * in->R0_12 + delta_T2 * in->R0_22;
111 R1_12 = in->R0_12 + delta_T * in->R0_22;
112 R1_22 = in->R0_22;
113 // Q = | T^3/3 T^2/2 | * drive_var
114 // | T^2/2 T |
115 R1_11 += in->drive_var * delta_T3 / 3;
116 R1_12 += in->drive_var * delta_T2 / 2;
117 R1_22 += in->drive_var * delta_T;
118
119 // inovation cov S1 = y_err + H*R1*H'
120 S1 = in->measurement_var + R1_11;
121 // filter gain W1 = (R1*H')/S1
122 W1_1 = R1_11 / S1;
123 W1_2 = R1_12 / S1;
124
125 // updated covariance R11 = R1 - W1*S1*W1'
126 R1_11 -= W1_1 * S1 * W1_1;
127 in->R0_11 = (R1_11 > 0) ? (unsigned short)R1_11 : 0;
128 R1_12 -= W1_1 * S1 * W1_2;
129 in->R0_12 = R1_12;
130 R1_22 -= W1_2 * S1 * W1_2;
131 in->R0_22 = (R1_22 > 0) ? (unsigned short)R1_22 : 0;
132 // updated state
133 range_hat += W1_1 * nu;
134 in->last_range = (range_hat > 0) ? (unsigned short)range_hat : 0;
135 range_rate_hat += W1_2 * nu;
136 in->last_range_rate = (signed short)range_rate_hat;
137 in->last_time = in->time;
138
139 return 0; // no errors
140 }
141 #endif
142
143 #endif /* CONFIG_WLS_CSI_PROC */
144
