126 | * Should be called after creation, unless position and velocity are assumed to be both zero.
127 | *
128 | * @param position
129 | * @param velocity
130 | * @param noise
131 | */
132 | public void setState(double position, double velocity, double noise) {
133 |
134 | // State vector
135 | mXa = position;
136 | mXb = velocity;
137 |
138 | // Covariance
139 | double n2 = noise * noise;
140 | mPa = n2 * mt4d4;
141 | mPb = n2 * mt3d2;
142 | mPc = mPb;
143 | mPd = n2 * mt2;
144 | }
145 |
146 | /**
147 | * Predict state.
148 | *
149 | * @param acceleration Should be 0 unless there's some sort of control input (a gas pedal, for instance).
150 | * @param timeMillisec The time the prediction is for.
151 | */
152 | public void predict(double acceleration, long timeMillisec) {
153 |
154 | long delta_t = timeMillisec - mPredTime;
155 | while (delta_t > TIME_STEP_MS) {
156 | mPredTime = mPredTime + TIME_STEP_MS;
157 |
158 | // x = F.x + G.u
159 | mXa = mXa + mXb * mt + acceleration * mt2d2;
160 | mXb = mXb + acceleration * mt;
161 |
162 | // P = F.P.F' + Q
163 | double Pdt = mPd * mt;
164 | double FPFtb = mPb + Pdt;
165 | double FPFta = mPa + mt * (mPc + FPFtb);
166 | double FPFtc = mPc + Pdt;
167 | double FPFtd = mPd;
168 |
169 | mPa = FPFta + mQa;
170 | mPb = FPFtb + mQb;
171 | mPc = FPFtc + mQc;
172 | mPd = FPFtd + mQd;
173 |
174 | delta_t = timeMillisec - mPredTime;
175 | }
176 | }
177 |
178 | /**
179 | * Update (correct) with the given measurement.
180 | *
181 | * @param position
182 | * @param noise
183 | */
184 | public void update(double position, double noise) {
185 |
186 | double r = noise * noise;
187 |
188 | // y = z - H . x
189 | double y = position - mXa;
190 |
191 | // S = H.P.H' + R
192 | double s = mPa + r;
193 | double si = 1.0 / s;
194 |
195 | // K = P.H'.S^(-1)
196 | double Ka = mPa * si;
197 | double Kb = mPc * si;
198 |
199 | // x = x + K.y
200 | mXa = mXa + Ka * y;
201 | mXb = mXb + Kb * y;
202 |
203 | // P = P - K.(H.P)
204 | double Pa = mPa - Ka * mPa;
205 | double Pb = mPb - Ka * mPb;
206 | double Pc = mPc - Kb * mPa;
207 | double Pd = mPd - Kb * mPb;
208 |
209 | mPa = Pa;
210 | mPb = Pb;
211 | mPc = Pc;
212 | mPd = Pd;
213 |
214 | }
215 |
216 | /**
217 | * @return Estimated position.
218 | */
219 | public double getPosition() {
220 | return mXa;
221 | }
222 |
223 | /**
224 | * @return Estimated velocity.
225 | */
226 | public double getVelocity() {
227 | return mXb;
228 | }
229 |
230 | /**
231 | * @return Accuracy
232 | */
233 | public double getAccuracy() {
234 | return Math.sqrt(mPd / mt2);
235 | }
236 | }
237 |
--------------------------------------------------------------------------------
/app/src/main/java/org/fitchfamily/android/dejavu/Observation.kt:
--------------------------------------------------------------------------------
1 | package org.fitchfamily.android.dejavu
2 |
3 | import org.fitchfamily.android.dejavu.BackendService.Companion.getCorrectedAsu
4 |
5 | /*
6 | * Local NLP Backend / DejaVu - A location provider backend for microG/UnifiedNlp
7 | *
8 | * Copyright (C) 2017 Tod Fitch
9 | * Copyright (C) 2022 Helium314
10 | *
11 | * This program is Free Software: you can redistribute it and/or modify
12 | * it under the terms of the GNU General Public License as
13 | * published by the Free Software Foundation, either version 3 of the
14 | * License, or (at your option) any later version.
15 | *
16 | * This program is distributed in the hope that it will be useful,
17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 | * GNU General Public License for more details.
20 | *
21 | * You should have received a copy of the GNU General Public License
22 | * along with this program. If not, see